diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
index 94c2946f758d1a387f49000c15d38082ab633877..5a7c01e37ef158ad16cf2350cb7ab03ecd8fd4be 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
@@ -22,20 +22,24 @@
#include "BasicControllers.h"
-#include <ArmarXCore/core/util/algorithm.h>
+#include <algorithm>
+
#include <ArmarXCore/core/logging/Logging.h>
-#include "util/CtrlUtil.h"
+#include <ArmarXCore/core/util/algorithm.h>
-#include <algorithm>
+#include "util/CtrlUtil.h"
namespace armarx
{
- float velocityControlWithAccelerationBounds(
- float dt, float maxDt,
- const float currentV, float targetV, float maxV,
- float acceleration, float deceleration,
- float directSetVLimit
- )
+ 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);
@@ -56,7 +60,7 @@ namespace armarx
}
//handle case 2 + 3
- const bool accelerate = sign(targetV) == sign(currentV) && // v in same direction
+ 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;
@@ -75,13 +79,20 @@ namespace armarx
////////////////////////////
//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)
+ 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)
{
@@ -110,14 +121,17 @@ namespace armarx
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 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 auto limit =
+ posIfBrakingNow <= positionLimitLoSoft ? positionLimitLoSoft : positionLimitHiSoft;
const float wayToGo = limit - currentPosition;
//decelerate!
@@ -134,7 +148,8 @@ namespace armarx
else
{
//handle 2-3
- nextv = velocityControlWithAccelerationBounds(dt, maxDt, currentV, targetV, maxV, acceleration, deceleration, directSetVLimit);
+ nextv = velocityControlWithAccelerationBounds(
+ dt, maxDt, currentV, targetV, maxV, acceleration, deceleration, directSetVLimit);
}
if (softLimitViolation == sign(nextv))
{
@@ -147,14 +162,16 @@ namespace armarx
return nextv;
}
-
- float positionThroughVelocityControlWithAccelerationBounds(
- float dt, float maxDt,
- float currentV, float maxV,
- float acceleration, float deceleration,
- float currentPosition, float targetPosition,
- float p
- )
+ 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);
@@ -173,15 +190,17 @@ namespace armarx
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 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;
+ 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 = std::clamp(signV * usedacc * dt, -maxDeltaV, maxDeltaV);
@@ -199,13 +218,18 @@ namespace armarx
}
}
- float positionThroughVelocityControlWithAccelerationAndPositionBounds(
- float dt, float maxDt,
- float currentV, float maxV,
- float acceleration, float deceleration,
- float currentPosition, float targetPosition,
- float p,
- float positionLimitLo, float positionLimitHi
+ float
+ positionThroughVelocityControlWithAccelerationAndPositionBounds(float dt,
+ float maxDt,
+ float currentV,
+ float maxV,
+ float acceleration,
+ float deceleration,
+ float currentPosition,
+ float targetPosition,
+ float p,
+ float positionLimitLo,
+ float positionLimitHi
)
{
@@ -224,7 +248,9 @@ namespace armarx
//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 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)
{
@@ -239,86 +265,102 @@ namespace armarx
//handle case 2-3
return positionThroughVelocityControlWithAccelerationBounds(
- dt, maxDt,
- currentV, maxV,
- acceleration, deceleration,
- currentPosition, std::clamp(targetPosition, positionLimitLo, positionLimitHi),
- p
- );
+ dt,
+ maxDt,
+ currentV,
+ maxV,
+ acceleration,
+ deceleration,
+ currentPosition,
+ std::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
+ 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)
+ float positionPeriodLo,
+ float positionPeriodHi)
{
currentPosition = periodicClamp(currentPosition, positionPeriodLo, positionPeriodHi);
- targetPosition = periodicClamp(targetPosition, 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
- )
+ 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
- );
+ 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);
+ const float positionPeriodLength = std::abs(positionPeriodHi - positionPeriodLo);
//we shift the positions such that 0 == target
- currentPosition = periodicClamp(currentPosition - targetPosition, 0.f, positionPeriodLength);
+ 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;
+ 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;
+ targetPosition = (overshoot - std::min(0.f, -direction)) *
+ positionPeriodLength; // - direction * pControlPosErrorLimit;
//move
- return positionThroughVelocityControlWithAccelerationBounds(
- dt, maxDt,
- currentV, maxV,
- acceleration, deceleration,
- currentPosition, targetPosition,
- p
- );
+ return positionThroughVelocityControlWithAccelerationBounds(dt,
+ maxDt,
+ currentV,
+ maxV,
+ acceleration,
+ deceleration,
+ currentPosition,
+ targetPosition,
+ p);
}
- bool VelocityControllerWithAccelerationBounds::validParameters() const
+ bool
+ VelocityControllerWithAccelerationBounds::validParameters() const
{
- return maxV > 0 &&
- acceleration > 0 &&
- deceleration > 0 &&
- targetV <= maxV &&
- targetV >= -maxV;
+ return maxV > 0 && acceleration > 0 && deceleration > 0 && targetV <= maxV &&
+ targetV >= -maxV;
}
- float VelocityControllerWithAccelerationBounds::run() const
+ float
+ VelocityControllerWithAccelerationBounds::run() const
{
const float useddt = std::min(std::abs(dt), std::abs(maxDt));
@@ -334,7 +376,7 @@ namespace armarx
}
//handle case 2 + 3
- const bool accelerate = sign(targetV) == sign(currentV) && // v in same direction
+ 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;
@@ -351,15 +393,14 @@ namespace armarx
return nextV;
}
- bool VelocityControllerWithRampedAcceleration::validParameters() const
+ bool
+ VelocityControllerWithRampedAcceleration::validParameters() const
{
- return maxV > 0 &&
- jerk > 0 &&
- targetV <= maxV &&
- targetV >= -maxV;
+ return maxV > 0 && jerk > 0 && targetV <= maxV && targetV >= -maxV;
}
- VelocityControllerWithRampedAcceleration::Output VelocityControllerWithRampedAcceleration::run() const
+ VelocityControllerWithRampedAcceleration::Output
+ VelocityControllerWithRampedAcceleration::run() const
{
const double useddt = std::min(std::abs(dt), std::abs(maxDt));
@@ -374,7 +415,7 @@ namespace armarx
{
// double nextAcc = (clampedTargetV - currentV) / useddt; // calculated acc is unstable!!!
// double nextJerk = (nextAcc - currentAcc) / useddt;
- Output result {clampedTargetV, 0, 0};
+ Output result{clampedTargetV, 0, 0};
return result;
}
@@ -393,14 +434,17 @@ namespace armarx
double t_to_zero_acc = std::abs(currentAcc) / jerk;
double v_at_t = ctrlutil::v(t_to_zero_acc, currentV, currentAcc, -goalDir * jerk);
- increaseAcc = math::MathUtils::Sign(v_at_t - clampedTargetV) != goalDir ;
+ increaseAcc = math::MathUtils::Sign(v_at_t - clampedTargetV) != goalDir;
// ARMARX_INFO << VAROUT(t_to_zero_acc) << VAROUT(v_at_t) << VAROUT(increaseAcc);
}
// v = a*a/(2j)
- const double adjustedJerk = std::abs(currentAcc * currentAcc / (2 * (clampedTargetV - currentV)));
- double usedJerk = increaseAcc ? goalDir * jerk :
- -goalDir *
- ((std::abs(currentV) > 0.01 && !std::isnan(adjustedJerk)) ? adjustedJerk : jerk);
+ const double adjustedJerk =
+ std::abs(currentAcc * currentAcc / (2 * (clampedTargetV - currentV)));
+ double usedJerk = increaseAcc
+ ? goalDir * jerk
+ : -goalDir * ((std::abs(currentV) > 0.01 && !std::isnan(adjustedJerk))
+ ? adjustedJerk
+ : jerk);
// double t_to_target_v = ctrlutil::t_to_v(clampedTargetV - currentV, currentAcc, usedJerk);
// double v_at_t2 = ctrlutil::v(t_to_target_v, currentV, currentAcc, usedJerk);
// ctrlutil::t_to_v(currentV, currentAcc, jerk);
@@ -415,18 +459,19 @@ namespace armarx
double nextAcc = (clampedTargetV - currentV) / useddt;
double nextJerk = (nextAcc - currentAcc) / useddt;
// ARMARX_INFO << "overshooting! " << VAROUT(clampedTargetV) << VAROUT(nextAcc) << VAROUT(nextJerk);
- Output result {clampedTargetV, nextAcc, nextJerk};
+ Output result{clampedTargetV, nextAcc, nextJerk};
return result;
}
// const double deltaVel = boost::algorithm::clamp(sign(currentV) * usedacc * useddt, -maxDeltaV, maxDeltaV);
const double nextV = ctrlutil::v(useddt, currentV, currentAcc, usedJerk);
const double nextAcc = ctrlutil::a(useddt, currentAcc, usedJerk);
// ARMARX_INFO << VAROUT(clampedTargetV) << VAROUT(nextV) << VAROUT(nextAcc) << VAROUT(usedJerk);
- Output result {nextV, nextAcc, usedJerk};
+ Output result{nextV, nextAcc, usedJerk};
return result;
}
- float VelocityControllerWithAccelerationBounds::estimateTime() const
+ float
+ VelocityControllerWithAccelerationBounds::estimateTime() const
{
const float curverror = std::abs(targetV - currentV);
if (curverror < directSetVLimit)
@@ -436,17 +481,17 @@ namespace armarx
return curverror / ((targetV > currentV) ? acceleration : deceleration);
}
-
-
- bool VelocityControllerWithAccelerationAndPositionBounds::validParameters() const
+ bool
+ VelocityControllerWithAccelerationAndPositionBounds::validParameters() const
{
- return VelocityControllerWithAccelerationBounds::validParameters() &&
- // positionLimitLoHard < positionLimitLoSoft &&
- positionLimitLoSoft < positionLimitHiSoft;
+ return VelocityControllerWithAccelerationBounds::validParameters() &&
+ // positionLimitLoHard < positionLimitLoSoft &&
+ positionLimitLoSoft < positionLimitHiSoft;
// positionLimitHiSoft < positionLimitHiHard;
}
- float VelocityControllerWithAccelerationAndPositionBounds::run() const
+ float
+ VelocityControllerWithAccelerationAndPositionBounds::run() const
{
// if (currentPosition <= positionLimitLoHard || currentPosition >= positionLimitHiHard)
// {
@@ -474,14 +519,18 @@ namespace armarx
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 brakingDist =
+ sign(currentV) * vsquared / 2.f /
+ std::abs(deceleration); //the braking distance points in the direction of the velocity
const float posIfBrakingNow = currentPosition + brakingDist;
// ARMARX_INFO << deactivateSpam(1) << VAROUT(currentPosition) << VAROUT(positionLimitLoSoft) << VAROUT(positionLimitHiSoft) << VAROUT(posIfBrakingNow);
- if ((posIfBrakingNow <= positionLimitLoSoft && currentV < 0) || (posIfBrakingNow >= positionLimitHiSoft && currentV > 0))
+ if ((posIfBrakingNow <= positionLimitLoSoft && currentV < 0) ||
+ (posIfBrakingNow >= positionLimitHiSoft && currentV > 0))
{
//case 1. -> brake now! (we try to have v=0 at the limit)
- const auto limit = posIfBrakingNow <= positionLimitLoSoft ? positionLimitLoSoft : positionLimitHiSoft;
+ const auto limit =
+ posIfBrakingNow <= positionLimitLoSoft ? positionLimitLoSoft : positionLimitHiSoft;
const float wayToGo = limit - currentPosition;
//decelerate!
@@ -501,7 +550,6 @@ namespace armarx
//handle 2-3
nextv = VelocityControllerWithAccelerationBounds::run();
// ARMARX_INFO << deactivateSpam(1) << "Desired target Vel: " << targetV << " " << VAROUT(nextv);
-
}
if (softLimitViolation == sign(nextv) && nextv != 0)
{
@@ -515,7 +563,8 @@ namespace armarx
return nextv;
}
- PositionThroughVelocityControllerWithAccelerationBounds::PositionThroughVelocityControllerWithAccelerationBounds()
+ PositionThroughVelocityControllerWithAccelerationBounds::
+ PositionThroughVelocityControllerWithAccelerationBounds()
{
#ifdef DEBUG_POS_CTRL
buffer = boost::circular_buffer<HelpStruct>(20);
@@ -524,7 +573,8 @@ namespace armarx
pid->threadSafe = false;
}
- float PositionThroughVelocityControllerWithAccelerationBounds::calculateProportionalGain() const
+ float
+ PositionThroughVelocityControllerWithAccelerationBounds::calculateProportionalGain() const
{
/* s = v_0*v_0/(2*a) -> sqrt(s*2*a) = v_0;
@@ -534,20 +584,17 @@ namespace armarx
return v_0 / pControlPosErrorLimit;
}
-
-
-
- bool PositionThroughVelocityControllerWithAccelerationBounds::validParameters() const
+ bool
+ PositionThroughVelocityControllerWithAccelerationBounds::validParameters() const
{
- return maxV > 0 &&
- acceleration > 0 &&
- deceleration > 0 &&
- // pControlPosErrorLimit > 0 &&
- // pControlVelLimit > 0 &&
- pid->Kp > 0;
+ return maxV > 0 && acceleration > 0 && deceleration > 0 &&
+ // pControlPosErrorLimit > 0 &&
+ // pControlVelLimit > 0 &&
+ pid->Kp > 0;
}
- float PositionThroughVelocityControllerWithAccelerationBounds::run() const
+ float
+ PositionThroughVelocityControllerWithAccelerationBounds::run() const
{
const float useddt = std::min(std::abs(dt), std::abs(maxDt));
const float signV = sign(currentV);
@@ -564,21 +611,24 @@ namespace armarx
float newTargetVelPController = pid->getControlValue();
//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 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 hardBrakingNeeded = std::abs(brakingDistance) > std::abs(positionError);
- const float safePositionError = (std::abs(positionError) < 0.0001) ? (sign(positionError) * 0.0001) : positionError;
- const float usedDeceleration = hardBrakingNeeded ?
- std::abs(currentV * currentV / 2.f / safePositionError) :
- deceleration;
+ const float safePositionError =
+ (std::abs(positionError) < 0.0001) ? (sign(positionError) * 0.0001) : positionError;
+ const float usedDeceleration = hardBrakingNeeded
+ ? std::abs(currentV * currentV / 2.f / safePositionError)
+ : deceleration;
const bool decelerate =
std::abs(currentV) > maxV || // we need to slow down (to stay in [-maxV,maxV]
hardBrakingNeeded ||
- sign(posErrorIfBrakingNow) != signV; // we are moving away from the target
+ sign(posErrorIfBrakingNow) != signV; // we are moving away from the target
- const float usedacc = decelerate ? -usedDeceleration : acceleration;
+ const float usedacc = decelerate ? -usedDeceleration : acceleration;
const float deltaVel = signV * usedacc * useddt;
float newTargetVelRampCtrl = std::clamp(currentV + deltaVel, -maxV, maxV);
bool PIDActive = /*std::abs(newTargetVelPController) < std::abs(newTargetVelRampCtrl)
@@ -589,10 +639,11 @@ namespace armarx
// ARMARX_INFO << "Switching to PID mode: " << VAROUT(positionError) << VAROUT(newTargetVelPController) << VAROUT(newTargetVelRampCtrl);
// }
this->currentlyPIDActive = PIDActive;
- float finalTargetVel = (currentlyPIDActive) ? newTargetVelPController : newTargetVelRampCtrl;
+ float finalTargetVel =
+ (currentlyPIDActive) ? newTargetVelPController : newTargetVelRampCtrl;
if (std::abs(positionError) < accuracy)
{
- return 0.0;// if close to target set zero velocity to avoid oscillating around target
+ return 0.0; // if close to target set zero velocity to avoid oscillating around target
}
// if (hardBrakingNeeded)
// {
@@ -604,10 +655,17 @@ namespace armarx
// VAROUT(usedacc) << VAROUT(deltaVel) << VAROUT(useddt);
// }
#ifdef DEBUG_POS_CTRL
- buffer.push_back({currentPosition, newTargetVelPController, newTargetVelRampCtrl, currentV, positionError, IceUtil::Time::now().toMicroSeconds()});
+ buffer.push_back({currentPosition,
+ newTargetVelPController,
+ newTargetVelRampCtrl,
+ currentV,
+ positionError,
+ IceUtil::Time::now().toMicroSeconds()});
// if (PIDModeActive != usePID)
- if (buffer.size() > 0 && sign(positionError) * sign(buffer[buffer.size() - 2].currentError) < 0 && eventHappeningCounter < 0)
+ if (buffer.size() > 0 &&
+ sign(positionError) * sign(buffer[buffer.size() - 2].currentError) < 0 &&
+ eventHappeningCounter < 0)
{
eventHappeningCounter = 10;
ARMARX_IMPORTANT << "HIGH VELOCITY DETECTED";
@@ -617,24 +675,30 @@ namespace armarx
ARMARX_IMPORTANT << "BUFFER CONTENT";
for (auto& elem : buffer)
{
- ARMARX_INFO << VAROUT(elem.currentPosition) << VAROUT(elem.targetVelocityPID) << VAROUT(elem.targetVelocityRAMP) << VAROUT(elem.currentV) << VAROUT(elem.currentError) << VAROUT(elem.timestamp);
+ ARMARX_INFO << VAROUT(elem.currentPosition) << VAROUT(elem.targetVelocityPID)
+ << VAROUT(elem.targetVelocityRAMP) << VAROUT(elem.currentV)
+ << VAROUT(elem.currentError) << VAROUT(elem.timestamp);
}
- ARMARX_IMPORTANT << VAROUT(newTargetVelPController) << VAROUT(newTargetVelRampCtrl) << VAROUT(currentPosition) << VAROUT(usePID) << VAROUT(positionError);
-
+ ARMARX_IMPORTANT << VAROUT(newTargetVelPController) << VAROUT(newTargetVelRampCtrl)
+ << VAROUT(currentPosition) << VAROUT(usePID) << VAROUT(positionError);
}
if (eventHappeningCounter >= 0)
{
eventHappeningCounter--;
}
- ARMARX_INFO << deactivateSpam(5) << VAROUT(newTargetVelPController) << VAROUT(newTargetVelRampCtrl) << VAROUT(currentPosition) << VAROUT(usePID) << VAROUT(positionError) << VAROUT(targetPosition) << VAROUT(currentV) << VAROUT(deltaVel) << VAROUT(useddt) << VAROUT(usedacc);
+ ARMARX_INFO << deactivateSpam(5) << VAROUT(newTargetVelPController)
+ << VAROUT(newTargetVelRampCtrl) << VAROUT(currentPosition) << VAROUT(usePID)
+ << VAROUT(positionError) << VAROUT(targetPosition) << VAROUT(currentV)
+ << VAROUT(deltaVel) << VAROUT(useddt) << VAROUT(usedacc);
PIDModeActive = usePID;
#endif
return finalTargetVel;
}
- float PositionThroughVelocityControllerWithAccelerationBounds::estimateTime() const
+ float
+ PositionThroughVelocityControllerWithAccelerationBounds::estimateTime() const
{
// const float brakingDistance = sign(currentV) * currentV * currentV / 2.f / deceleration;
const float posError = targetPosition - currentPosition;
@@ -649,7 +713,7 @@ namespace armarx
float t = 0;
float curVel = currentV;
float curPos = currentPosition;
- auto curPosEr = [&] {return targetPosition - curPos;};
+ auto curPosEr = [&] { return targetPosition - curPos; };
// auto curBrake = [&] {return std::abs(brakingDistance(curVel, deceleration));};
//check for overshooting of target
@@ -671,24 +735,27 @@ namespace armarx
// }
//curBrake()<=curPosEr() && curVel <= maxV
// auto tr = trapeze(curVel, acceleration, maxV, deceleration, 0, curPosEr());
- auto tr = trapeze(curVel, std::abs(curVel) > maxV ? deceleration : acceleration, maxV, deceleration, 0, curPosEr());
+ auto tr = trapeze(curVel,
+ std::abs(curVel) > maxV ? deceleration : acceleration,
+ maxV,
+ deceleration,
+ 0,
+ curPosEr());
return t + tr.at(0).dt + tr.at(1).dt + tr.at(2).dt;
-
-
}
- bool PositionThroughVelocityControllerWithAccelerationAndPositionBounds::validParameters() const
+ bool
+ PositionThroughVelocityControllerWithAccelerationAndPositionBounds::validParameters() const
{
- return PositionThroughVelocityControllerWithAccelerationBounds::validParameters() &&
- ! std::isnan(positionLimitLo) &&
- ! std::isnan(positionLimitHi) &&
- positionLimitLo < positionLimitHi &&
- targetPosition <= positionLimitHi &&
- positionLimitLo <= targetPosition;
+ return PositionThroughVelocityControllerWithAccelerationBounds::validParameters() &&
+ !std::isnan(positionLimitLo) && !std::isnan(positionLimitHi) &&
+ positionLimitLo < positionLimitHi && targetPosition <= positionLimitHi &&
+ positionLimitLo <= targetPosition;
}
- float PositionThroughVelocityControllerWithAccelerationAndPositionBounds::run() const
+ float
+ PositionThroughVelocityControllerWithAccelerationAndPositionBounds::run() const
{
const float useddt = std::min(std::abs(dt), std::abs(maxDt));
const float signV = sign(currentV);
@@ -701,7 +768,9 @@ namespace armarx
// 5. we need to decelerate (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 brakingDistance =
+ signV * vsquared / 2.f /
+ deceleration; //the braking distance points in the direction of the velocity
const float posIfBrakingNow = currentPosition + brakingDistance;
float direction = targetPosition < currentPosition ? -1 : 1;
// ARMARX_INFO << deactivateSpam(0.5) << VAROUT(direction) << VAROUT(brakingDistance) << VAROUT(currentPosition) << VAROUT(targetPosition);
@@ -722,11 +791,12 @@ namespace armarx
return PositionThroughVelocityControllerWithAccelerationBounds::run();
}
- std::array<deltas, 3> trapeze(float v0, float acc, float vMax, float dec, float vt, float dx)
+ std::array<deltas, 3>
+ trapeze(float v0, float acc, float vMax, float dec, float vt, float dx)
{
- acc = std::abs(acc);
+ acc = std::abs(acc);
vMax = std::abs(vMax);
- dec = std::abs(dec);
+ dec = std::abs(dec);
auto calc = [&](float vmax)
{
const deltas dacc = accelerateToVelocity(v0, acc, vmax);
@@ -740,7 +810,7 @@ namespace armarx
mid.dx = dxMax - dx;
mid.dv = vMax;
mid.dt = std::abs(mid.dx / mid.dv);
- return std::make_pair(true, std::array<deltas, 3> {dacc, mid, ddec});
+ 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}
@@ -758,9 +828,11 @@ namespace armarx
// -> 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 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);
@@ -778,7 +850,7 @@ namespace armarx
mid.dx = 0;
mid.dv = vx;
mid.dt = 0;
- return std::make_pair(true, std::array<deltas, 3> {daccvx, mid, ddecvx});
+ return std::make_pair(true, std::array<deltas, 3>{daccvx, mid, ddecvx});
}
case 2:
{
@@ -792,20 +864,21 @@ namespace armarx
if (daccvx1.dt + ddecvx1.dt < daccvx2.dt + ddecvx2.dt)
{
mid.dv = vxs.first;
- return std::make_pair(true, std::array<deltas, 3> {daccvx1, mid, ddecvx1});
+ return std::make_pair(true,
+ std::array<deltas, 3>{daccvx1, mid, ddecvx1});
}
mid.dv = vxs.second;
- return std::make_pair(true, std::array<deltas, 3> {daccvx2, mid, ddecvx2});
+ return std::make_pair(true, std::array<deltas, 3>{daccvx2, mid, ddecvx2});
}
default:
- throw std::logic_error {"unreachable code (bool + bool neither 0,1,2)"};
+ 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);
+ const auto negVMax = calc(-vMax);
switch (plusVMax.first + negVMax.first)
{
case 0:
@@ -814,26 +887,26 @@ namespace armarx
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;
+ 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)"};
+ throw std::logic_error{"unreachable code (bool + bool neither 0,1,2)"};
}
}
-
-
-
-
- float PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition::run() const
+ 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
+ PositionThroughVelocityControllerWithAccelerationBounds sub =
+ *this; // slice parent class from this
sub.currentPosition = periodicClamp(currentPosition, positionPeriodLo, positionPeriodHi);
// ARMARX_INFO << deactivateSpam(1) << VAROUT(currentPosition) << VAROUT(sub.currentPosition) << VAROUT(positionPeriodLo) << VAROUT(positionPeriodHi);
- sub.targetPosition = periodicClamp(targetPosition, positionPeriodLo, positionPeriodHi);
+ sub.targetPosition = periodicClamp(targetPosition, positionPeriodLo, positionPeriodHi);
const float brakingDist = brakingDistance(currentV, deceleration);
// const float posIfBrakingNow = currentPosition + brakingDist;
@@ -850,10 +923,12 @@ namespace armarx
//we transform the problem with periodic bounds into
//a problem with position bounds
- const float positionPeriodLength = std::abs(positionPeriodHi - positionPeriodLo);
+ const float positionPeriodLength = std::abs(positionPeriodHi - positionPeriodLo);
//we shift the positions such that 0 == target
- sub.currentPosition = periodicClamp(currentPosition - targetPosition, -positionPeriodLength * 0.5f, positionPeriodLength * 0.5f);
+ sub.currentPosition = periodicClamp(currentPosition - targetPosition,
+ -positionPeriodLength * 0.5f,
+ positionPeriodLength * 0.5f);
//how many times will we go over the target if we simply brake now?
const float overshoot = std::trunc((currentPosition + brakingDist) / positionPeriodLength);
@@ -862,27 +937,34 @@ namespace armarx
if (direction == 0)
{
//determine the direction to go (1 == pos vel, -1 == neg vel)
- direction = (periodicClamp(currentPosition + brakingDist, -positionPeriodLength * 0.5f, positionPeriodLength * 0.5f) <= 0) ? 1 : -1;
+ direction = (periodicClamp(currentPosition + brakingDist,
+ -positionPeriodLength * 0.5f,
+ positionPeriodLength * 0.5f) <= 0)
+ ? 1
+ : -1;
}
//shift the target away from 0
- sub.targetPosition = (overshoot * -direction) * positionPeriodLength; // - direction * pControlPosErrorLimit;
+ sub.targetPosition =
+ (overshoot * -direction) * positionPeriodLength; // - direction * pControlPosErrorLimit;
//move
return sub.run();
}
- double PositionThroughVelocityControllerWithAccelerationRamps::getTargetPosition() const
+ double
+ PositionThroughVelocityControllerWithAccelerationRamps::getTargetPosition() const
{
return targetPosition;
}
- bool PositionThroughVelocityControllerWithAccelerationBounds::getCurrentlyPIDActive() const
+ bool
+ PositionThroughVelocityControllerWithAccelerationBounds::getCurrentlyPIDActive() const
{
return currentlyPIDActive;
}
-
- void PositionThroughVelocityControllerWithAccelerationRamps::setTargetPosition(double value)
+ void
+ PositionThroughVelocityControllerWithAccelerationRamps::setTargetPosition(double value)
{
if (std::abs(value - targetPosition) > 0.0001)
{
@@ -891,23 +973,22 @@ namespace armarx
targetPosition = value;
}
- PositionThroughVelocityControllerWithAccelerationRamps::PositionThroughVelocityControllerWithAccelerationRamps()
+ PositionThroughVelocityControllerWithAccelerationRamps::
+ PositionThroughVelocityControllerWithAccelerationRamps()
{
-
}
- bool PositionThroughVelocityControllerWithAccelerationRamps::validParameters() const
+ bool
+ PositionThroughVelocityControllerWithAccelerationRamps::validParameters() const
{
- return maxV > 0 &&
- acceleration > 0 &&
- deceleration > 0 &&
- jerk > 0 &&
- // pControlPosErrorLimit > 0 &&
- // pControlVelLimit > 0 &&
- p > 0;
+ return maxV > 0 && acceleration > 0 && deceleration > 0 && jerk > 0 &&
+ // pControlPosErrorLimit > 0 &&
+ // pControlVelLimit > 0 &&
+ p > 0;
}
- PositionThroughVelocityControllerWithAccelerationRamps::Output PositionThroughVelocityControllerWithAccelerationRamps::run()
+ PositionThroughVelocityControllerWithAccelerationRamps::Output
+ PositionThroughVelocityControllerWithAccelerationRamps::run()
{
PositionThroughVelocityControllerWithAccelerationRamps::Output result;
const double useddt = std::min(std::abs(dt), std::abs(maxDt));
@@ -940,34 +1021,48 @@ namespace armarx
// double newAcceleration = output.acceleration;
// ARMARX_INFO << "state: " << (int)state << " new state: " << (int)newState;
state = newState;
- const double brakingDistance = brData.dt2 >= 0 ? brData.s_total : signV * currentV * currentV / 2.f / deceleration; //the braking distance points in the direction of the velocity
+ const double brakingDistance =
+ brData.dt2 >= 0
+ ? brData.s_total
+ : signV * currentV * currentV / 2.f /
+ deceleration; //the braking distance points in the direction of the velocity
const double posIfBrakingNow = currentPosition + brakingDistance;
const double posErrorIfBrakingNow = targetPosition - posIfBrakingNow;
const bool hardBrakingNeeded = std::abs(brakingDistance) > std::abs(positionError);
// const double usedJerk = jerk;//currentState==State::DecAccDecJerk?std::abs(ctrlutil::jerk(ctrlutil::t_to_v(currentV, -signV*currentAcc, signV*jerk), positionError, currentV, currentAcc))
// :jerk;
- const double jerkDir = (newState == State::DecAccIncJerk || newState == State::IncAccDecJerk) ? -1.0f : 1.0f;
+ const double jerkDir =
+ (newState == State::DecAccIncJerk || newState == State::IncAccDecJerk) ? -1.0f : 1.0f;
const double usedJerk = goalDir * jerkDir * usedAbsJerk;
- const double deltaAcc = usedJerk * useddt;
- const double newAcceleration = (newState == State::Keep) ? currentAcc : (newState == State::DecAccDecJerk ? output.acceleration : currentAcc + deltaAcc);
+ const double deltaAcc = usedJerk * useddt;
+ const double newAcceleration =
+ (newState == State::Keep)
+ ? currentAcc
+ : (newState == State::DecAccDecJerk ? output.acceleration : currentAcc + deltaAcc);
result.acceleration = newAcceleration;
result.jerk = usedJerk;
- const double usedDeceleration = hardBrakingNeeded ?
- -std::abs(currentV * currentV / 2.f / math::MathUtils::LimitTo(positionError, 0.0001)) :
- newAcceleration;
- (void) usedDeceleration;
+ const double usedDeceleration =
+ hardBrakingNeeded ? -std::abs(currentV * currentV / 2.f /
+ math::MathUtils::LimitTo(positionError, 0.0001))
+ : newAcceleration;
+ (void)usedDeceleration;
const bool decelerate =
std::abs(currentV) > maxV || // we need to slow down (to stay in [-maxV,maxV]
hardBrakingNeeded ||
- sign(posErrorIfBrakingNow) != signV; // we are moving away from the target
- (void) decelerate;
-
- const double deltaVel = ctrlutil::v(useddt, 0, newAcceleration, usedJerk);//signV * newAcceleration * useddt;
- (void) deltaVel;
-
- double newTargetVelRampCtrl = ctrlutil::v(useddt, currentV, currentAcc, usedJerk);//boost::algorithm::clamp(currentV + deltaVel, -maxV, maxV);
+ sign(posErrorIfBrakingNow) != signV; // we are moving away from the target
+ (void)decelerate;
+
+ const double deltaVel =
+ ctrlutil::v(useddt, 0, newAcceleration, usedJerk); //signV * newAcceleration * useddt;
+ (void)deltaVel;
+
+ double newTargetVelRampCtrl =
+ ctrlutil::v(useddt,
+ currentV,
+ currentAcc,
+ usedJerk); //boost::algorithm::clamp(currentV + deltaVel, -maxV, maxV);
newTargetVelRampCtrl = std::clamp(newTargetVelRampCtrl, -maxV, maxV);
bool usePID = std::abs(newTargetVelPController) < std::abs(newTargetVelRampCtrl); //||
// std::abs(newTargetVelPController) < pControlVelLimit &&
@@ -979,18 +1074,27 @@ namespace armarx
state = State::PCtrl;
}
double finalTargetVel = usePID ? newTargetVelPController : newTargetVelRampCtrl;
- ARMARX_INFO << VAROUT(usePID) << VAROUT(result.jerk) << VAROUT(newTargetVelPController) << VAROUT(currentV);
+ ARMARX_INFO << VAROUT(usePID) << VAROUT(result.jerk) << VAROUT(newTargetVelPController)
+ << VAROUT(currentV);
// ARMARX_INFO << VAROUT(currentV) << VAROUT(currentAcc) << VAROUT(usedJerk) << VAROUT(newAcceleration) << VAROUT(deltaVel) << VAROUT(finalTargetVel) << " new distance: " << (targetPosition - ctrlutil::s(dt, currentPosition, currentV, currentAcc, usedJerk));
if (std::abs(positionError) < accuracy)
{
- finalTargetVel = 0.0f; // if close to target set zero velocity to avoid oscillating around target
+ finalTargetVel =
+ 0.0f; // if close to target set zero velocity to avoid oscillating around target
}
#ifdef DEBUG_POS_CTRL
- buffer.push_back({currentPosition, newTargetVelPController, newTargetVelRampCtrl, currentV, positionError, IceUtil::Time::now().toMicroSeconds()});
+ buffer.push_back({currentPosition,
+ newTargetVelPController,
+ newTargetVelRampCtrl,
+ currentV,
+ positionError,
+ IceUtil::Time::now().toMicroSeconds()});
// if (PIDModeActive != usePID)
- if (buffer.size() > 0 && sign(positionError) * sign(buffer[buffer.size() - 2].currentError) < 0 && eventHappeningCounter < 0)
+ if (buffer.size() > 0 &&
+ sign(positionError) * sign(buffer[buffer.size() - 2].currentError) < 0 &&
+ eventHappeningCounter < 0)
{
eventHappeningCounter = 10;
ARMARX_IMPORTANT << "HIGH VELOCITY DETECTED";
@@ -1000,30 +1104,37 @@ namespace armarx
ARMARX_IMPORTANT << "BUFFER CONTENT";
for (auto& elem : buffer)
{
- ARMARX_INFO << VAROUT(elem.currentPosition) << VAROUT(elem.targetVelocityPID) << VAROUT(elem.targetVelocityRAMP) << VAROUT(elem.currentV) << VAROUT(elem.currentError) << VAROUT(elem.timestamp);
+ ARMARX_INFO << VAROUT(elem.currentPosition) << VAROUT(elem.targetVelocityPID)
+ << VAROUT(elem.targetVelocityRAMP) << VAROUT(elem.currentV)
+ << VAROUT(elem.currentError) << VAROUT(elem.timestamp);
}
- ARMARX_IMPORTANT << VAROUT(newTargetVelPController) << VAROUT(newTargetVelRampCtrl) << VAROUT(currentPosition) << VAROUT(usePID) << VAROUT(positionError);
-
+ ARMARX_IMPORTANT << VAROUT(newTargetVelPController) << VAROUT(newTargetVelRampCtrl)
+ << VAROUT(currentPosition) << VAROUT(usePID) << VAROUT(positionError);
}
if (eventHappeningCounter >= 0)
{
eventHappeningCounter--;
}
- ARMARX_INFO << deactivateSpam(5) << VAROUT(newTargetVelPController) << VAROUT(newTargetVelRampCtrl) << VAROUT(currentPosition) << VAROUT(usePID) << VAROUT(positionError) << VAROUT(targetPosition) << VAROUT(currentV) << VAROUT(deltaVel) << VAROUT(useddt) << VAROUT(usedacc);
+ ARMARX_INFO << deactivateSpam(5) << VAROUT(newTargetVelPController)
+ << VAROUT(newTargetVelRampCtrl) << VAROUT(currentPosition) << VAROUT(usePID)
+ << VAROUT(positionError) << VAROUT(targetPosition) << VAROUT(currentV)
+ << VAROUT(deltaVel) << VAROUT(useddt) << VAROUT(usedacc);
PIDModeActive = usePID;
#endif
result.velocity = finalTargetVel;
return result;
}
- double PositionThroughVelocityControllerWithAccelerationRamps::estimateTime() const
+ double
+ PositionThroughVelocityControllerWithAccelerationRamps::estimateTime() const
{
throw LocalException("NYI");
return 0;
}
- double PositionThroughVelocityControllerWithAccelerationRamps::calculateProportionalGain() const
+ double
+ PositionThroughVelocityControllerWithAccelerationRamps::calculateProportionalGain() const
{
/* s = v_0*v_0/(2*a) -> sqrt(s*2*a) = v_0;
@@ -1034,7 +1145,8 @@ namespace armarx
}
std::pair<PositionThroughVelocityControllerWithAccelerationRamps::State,
- PositionThroughVelocityControllerWithAccelerationRamps::Output> PositionThroughVelocityControllerWithAccelerationRamps::calcState() const
+ PositionThroughVelocityControllerWithAccelerationRamps::Output>
+ PositionThroughVelocityControllerWithAccelerationRamps::calcState() const
{
// auto integratedChange = [](v0)
@@ -1048,20 +1160,29 @@ namespace armarx
const auto brData = ctrlutil::braking_distance_with_wedge(currentV, currentAcc, newJerk);
const auto goalDir = math::MathUtils::Sign(targetPosition - currentPosition);
const auto curVDir = math::MathUtils::Sign(currentV);
- const auto straightBrakingDistance = ctrlutil::brakingDistance(currentV, currentAcc, newJerk);
+ const auto straightBrakingDistance =
+ ctrlutil::brakingDistance(currentV, currentAcc, newJerk);
const double brakingDistance = brData.dt2 < 0 ? straightBrakingDistance : brData.s_total;
const double distance = std::abs(targetPosition - currentPosition);
- const double t_to_stop = ctrlutil::t_to_v(currentV, -curVDir * currentAcc, curVDir * newJerk);
- const auto calculatedJerk = std::abs(ctrlutil::jerk(t_to_stop, distance, currentV, currentAcc));
+ const double t_to_stop =
+ ctrlutil::t_to_v(currentV, -curVDir * currentAcc, curVDir * newJerk);
+ const auto calculatedJerk =
+ std::abs(ctrlutil::jerk(t_to_stop, distance, currentV, currentAcc));
double tmp_t, tmp_acc, tmp_jerk;
// std::tie( tmp_t, tmp_acc, tmp_jerk) = ctrlutil::calcAccAndJerk(targetPosition-(currentPosition+brData.s1), brData.v1);
- std::tie(tmp_t, tmp_acc, tmp_jerk) = ctrlutil::calcAccAndJerk(targetPosition - currentPosition, currentV);
- const double posWhenBrakingWithCustomJerk = ctrlutil::s(tmp_t, currentPosition + brData.s1, brData.v1, tmp_acc, curVDir * tmp_jerk);
+ std::tie(tmp_t, tmp_acc, tmp_jerk) =
+ ctrlutil::calcAccAndJerk(targetPosition - currentPosition, currentV);
+ const double posWhenBrakingWithCustomJerk =
+ ctrlutil::s(tmp_t, currentPosition + brData.s1, brData.v1, tmp_acc, curVDir * tmp_jerk);
// const double brakingDistanceWithCustomJerk = ctrlutil::s(tmp_t, 0, currentV, tmp_acc, curVDir * tmp_jerk);
- const double posWhenBrakingWithFixedJerk = ctrlutil::s(brData.dt2, currentPosition + brData.s1, brData.v1, brData.a1, curVDir * jerk);
- (void) calculatedJerk, (void) posWhenBrakingWithCustomJerk, (void) posWhenBrakingWithFixedJerk;
+ const double posWhenBrakingWithFixedJerk = ctrlutil::s(
+ brData.dt2, currentPosition + brData.s1, brData.v1, brData.a1, curVDir * jerk);
+ (void)calculatedJerk, (void)posWhenBrakingWithCustomJerk, (void)posWhenBrakingWithFixedJerk;
- ARMARX_INFO << VAROUT((int)state) << VAROUT(distance) << VAROUT(brakingDistance);//VAROUT(straightBrakingDistance) << VAROUT(brData.dt1) << VAROUT(brData.dt2) << VAROUT(brData.s_total) << VAROUT(calculatedJerk);
+ ARMARX_INFO
+ << VAROUT((int)state) << VAROUT(distance)
+ << VAROUT(
+ brakingDistance); //VAROUT(straightBrakingDistance) << VAROUT(brData.dt1) << VAROUT(brData.dt2) << VAROUT(brData.s_total) << VAROUT(calculatedJerk);
// if(brData.dt2 < 0)
// {
@@ -1074,8 +1195,10 @@ namespace armarx
// return;
// }
if (state < State::DecAccIncJerk &&
- ((distance > brakingDistance && (math::MathUtils::Sign(currentAcc) == curVDir || std::abs(currentAcc) < 0.1)) // we are accelerating
- || distance > brakingDistance * 2 // we are decelerating -> dont switch to accelerating to easily
+ ((distance > brakingDistance && (math::MathUtils::Sign(currentAcc) == curVDir ||
+ std::abs(currentAcc) < 0.1)) // we are accelerating
+ || distance > brakingDistance *
+ 2 // we are decelerating -> dont switch to accelerating to easily
|| curVDir != goalDir)) // we are moving away from goal
{
if (std::abs(maxV - currentV) < 0.1 && std::abs(currentAcc) < 0.1)
@@ -1085,19 +1208,19 @@ namespace armarx
return std::make_pair(newState, result);
}
// calculate if we need to increase or decrease acc
- double t_to_max_v = ctrlutil::t_to_v(goalDir * maxV - currentV, currentAcc, goalDir * newJerk);
+ double t_to_max_v =
+ ctrlutil::t_to_v(goalDir * maxV - currentV, currentAcc, goalDir * newJerk);
double acc_at_t = ctrlutil::a(t_to_max_v, std::abs(currentAcc), -newJerk);
if (acc_at_t < 0.1)
{
- newState = State::IncAccIncJerk;
+ newState = State::IncAccIncJerk;
return std::make_pair(newState, result);
}
else
{
- newState = State::IncAccDecJerk;
+ newState = State::IncAccDecJerk;
return std::make_pair(newState, result);
}
-
}
else
{
@@ -1106,41 +1229,44 @@ namespace armarx
double tmpV = ctrlutil::v(t_to_0, 0, currentAcc, -curVDir * newJerk);
double vAfterBraking = ctrlutil::v(t_to_0, currentV, currentAcc, -curVDir * newJerk);
double accAfterBraking = ctrlutil::a(t_to_0, currentAcc, curVDir * newJerk);
- const double posWhenBrakingNow = ctrlutil::s(t_to_0, currentPosition, currentV, currentAcc, -curVDir * newJerk);
- const double simpleBrakingDistance = ctrlutil::s(t_to_0, 0, currentV, currentAcc, -curVDir * newJerk);
- (void) tmpV, (void) vAfterBraking, (void) accAfterBraking, (void) posWhenBrakingNow, (void) simpleBrakingDistance;
+ const double posWhenBrakingNow =
+ ctrlutil::s(t_to_0, currentPosition, currentV, currentAcc, -curVDir * newJerk);
+ const double simpleBrakingDistance =
+ ctrlutil::s(t_to_0, 0, currentV, currentAcc, -curVDir * newJerk);
+ (void)tmpV, (void)vAfterBraking, (void)accAfterBraking, (void)posWhenBrakingNow,
+ (void)simpleBrakingDistance;
double acc_at_t = ctrlutil::a(t_to_0, std::abs(currentAcc), -newJerk);
- std::tie(t, a0, newJerk) = ctrlutil::calcAccAndJerk(targetPosition - currentPosition, currentV);
+ std::tie(t, a0, newJerk) =
+ ctrlutil::calcAccAndJerk(targetPosition - currentPosition, currentV);
double at = ctrlutil::a(t, a0, newJerk);
double vt = ctrlutil::v(t, currentV, a0, newJerk);
double st = ctrlutil::s(t, targetPosition - currentPosition, currentV, a0, newJerk);
- (void) at, (void) vt, (void) st;
+ (void)at, (void)vt, (void)st;
if (this->state <= State::Keep)
{
- newState = State::DecAccIncJerk;
+ newState = State::DecAccIncJerk;
return std::make_pair(newState, result);
}
- else if ((state == State::DecAccIncJerk && acc_at_t > 1) || state == State::DecAccDecJerk)
+ else if ((state == State::DecAccIncJerk && acc_at_t > 1) ||
+ state == State::DecAccDecJerk)
{
result.jerk = newJerk;
result.acceleration = currentAcc + (a0 - currentAcc) * 0.5 + dt * newJerk;
- newState = State::DecAccDecJerk;
+ newState = State::DecAccDecJerk;
return std::make_pair(newState, result);
}
else
{
return std::make_pair(newState, result);
}
-
}
throw LocalException();
}
-
-
- void MinJerkPositionController::reset()
+ void
+ MinJerkPositionController::reset()
{
currentTime = 0;
@@ -1150,7 +1276,8 @@ namespace armarx
currentP_Phase3 = -1;
}
- double MinJerkPositionController::getTargetPosition() const
+ double
+ MinJerkPositionController::getTargetPosition() const
{
return targetPosition;
}
@@ -1245,8 +1372,6 @@ namespace armarx
// }
-
-
// double jerk = min_jerk_calc_jerk(remainingTime, currentPosition, currentV, currentAcc, targetPosition);
// double newAcc = currentAcc + jerk * dt;
// double newVelocity = ctrlutil::v(dt, currentV, currentAcc, jerk);
@@ -1296,17 +1421,18 @@ namespace armarx
// }
- bool VelocityControllerWithRampedAccelerationAndPositionBounds::validParameters() const
+ bool
+ VelocityControllerWithRampedAccelerationAndPositionBounds::validParameters() const
{
return VelocityControllerWithRampedAcceleration::validParameters() &&
- positionLimitHiSoft > positionLimitLoSoft &&
- deceleration > 0;
+ positionLimitHiSoft > positionLimitLoSoft && deceleration > 0;
}
- VelocityControllerWithRampedAcceleration::Output VelocityControllerWithRampedAccelerationAndPositionBounds::run() const
+ VelocityControllerWithRampedAcceleration::Output
+ VelocityControllerWithRampedAccelerationAndPositionBounds::run() const
{
const double useddt = std::min(std::abs(dt), std::abs(maxDt));
- (void) useddt;
+ (void)useddt;
// if (currentPosition <= positionLimitLoHard || currentPosition >= positionLimitHiHard)
// {
@@ -1334,14 +1460,18 @@ namespace armarx
double nextv;
//handle case 1
const double vsquared = currentV * currentV;
- const double brakingDist = sign(currentV) * vsquared / 2.f / std::abs(deceleration); //the braking distance points in the direction of the velocity
+ const double brakingDist =
+ sign(currentV) * vsquared / 2.f /
+ std::abs(deceleration); //the braking distance points in the direction of the velocity
const double posIfBrakingNow = currentPosition + brakingDist;
// ARMARX_INFO << deactivateSpam(0.1) << VAROUT(currentPosition) << VAROUT(positionLimitLoSoft) << VAROUT(positionLimitHiSoft) << VAROUT(posIfBrakingNow) << VAROUT(currentV);
- if ((posIfBrakingNow <= positionLimitLoSoft && currentV < 0) || (posIfBrakingNow >= positionLimitHiSoft && currentV > 0))
+ if ((posIfBrakingNow <= positionLimitLoSoft && currentV < 0) ||
+ (posIfBrakingNow >= positionLimitHiSoft && currentV > 0))
{
//case 1. -> brake now! (we try to have v=0 at the limit)
- const auto limit = posIfBrakingNow <= positionLimitLoSoft ? positionLimitLoSoft : positionLimitHiSoft;
+ const auto limit =
+ posIfBrakingNow <= positionLimitLoSoft ? positionLimitLoSoft : positionLimitHiSoft;
const double wayToGo = limit - currentPosition;
//decelerate!
@@ -1361,7 +1491,6 @@ namespace armarx
//handle 2-3
return VelocityControllerWithRampedAcceleration::run();
// ARMARX_INFO << deactivateSpam(1) << "Desired target Vel: " << targetV << " " << VAROUT(nextv);
-
}
if (softLimitViolation == sign(nextv) && nextv != 0)
{
@@ -1373,12 +1502,11 @@ namespace armarx
// double nextAcc = (nextv - currentV) / useddt; // doesnt work with calculated acc!!
// double nextJerk = (nextAcc - currentAcc) / useddt;
//the next velocity will not violate the pos bounds harder than they are already
- return Output {nextv, 0, 0};
+ return Output{nextv, 0, 0};
}
-
-
- void MinJerkPositionController::setTargetPosition(double newTargetPosition)
+ void
+ MinJerkPositionController::setTargetPosition(double newTargetPosition)
{
if (std::abs(targetPosition - newTargetPosition) > 0.0001)
{
@@ -1387,10 +1515,11 @@ namespace armarx
double rawFinishTime = 0.0;
double decelerationTime = std::abs(currentV / desiredDeceleration);
// calculate the time it should take to reach the goal (ToDo: find exact time)
- if (math::MathUtils::Sign(currentV) == math::MathUtils::Sign(distance)
- && std::abs(currentV) > 0.0
- && ctrlutil::s(decelerationTime, 0, std::abs(currentV), -desiredDeceleration, 0) > std::abs(distance))
- // && ctrlutil::brakingDistance(currentV, currentAcc, desiredJerk) < distance)
+ if (math::MathUtils::Sign(currentV) == math::MathUtils::Sign(distance) &&
+ std::abs(currentV) > 0.0 &&
+ ctrlutil::s(decelerationTime, 0, std::abs(currentV), -desiredDeceleration, 0) >
+ std::abs(distance))
+ // && ctrlutil::brakingDistance(currentV, currentAcc, desiredJerk) < distance)
{
// ARMARX_INFO << "branch 1: " << VAROUT(currentV) << VAROUT(desiredDeceleration) << VAROUT(distance);
rawFinishTime = decelerationTime;
@@ -1398,11 +1527,14 @@ namespace armarx
else
{
// ARMARX_INFO << "branch 2: " << VAROUT(currentV) << VAROUT(desiredDeceleration) << VAROUT(distance);
- double accelerationTime = std::abs((math::MathUtils::Sign(distance) * maxV - currentV) / desiredDeceleration);
+ double accelerationTime = std::abs(
+ (math::MathUtils::Sign(distance) * maxV - currentV) / desiredDeceleration);
double decelerationTime = std::abs(maxV / desiredDeceleration);
- rawFinishTime = std::max(decelerationTime + accelerationTime, std::abs(distance / (maxV * 0.75)));
+ rawFinishTime = std::max(decelerationTime + accelerationTime,
+ std::abs(distance / (maxV * 0.75)));
}
- double estimatedTime = ctrlutil::t_to_v_with_wedge_acc(std::abs(distance), std::abs(currentV), std::abs(desiredDeceleration));
+ double estimatedTime = ctrlutil::t_to_v_with_wedge_acc(
+ std::abs(distance), std::abs(currentV), std::abs(desiredDeceleration));
double minTime = 0.8;
if (!std::isnan(estimatedTime))
{
@@ -1414,22 +1546,27 @@ namespace armarx
}
targetPosition = newTargetPosition;
// ARMARX_INFO << "New finish time: " << finishTime << " " << VAROUT(rawFinishTime) << VAROUT(estimatedTime);
-
}
-
-
}
- MinJerkPositionController::Output MinJerkPositionController::run()
+ MinJerkPositionController::Output
+ MinJerkPositionController::run()
{
- auto min_jerk_calc_jerk = [&](double tf, double s0, double v0, double a0, double xf = 0, double t0 = 0.0)
+ auto min_jerk_calc_jerk =
+ [&](double tf, double s0, double v0, double a0, double xf = 0, double t0 = 0.0)
{
double D = tf - t0;
return 60.0 / (D * D * D) * (xf - s0) - 36.0 / (D * D) * v0 - 9.0 / D * a0;
};
- auto min_jerk = [&](double t, double tf, double s0, double v0, double a0, double xf = 0, double t0 = 0.0)
+ auto min_jerk = [&](double t,
+ double tf,
+ double s0,
+ double v0,
+ double a0,
+ double xf = 0,
+ double t0 = 0.0)
{
double D = tf - t0;
ARMARX_CHECK(D != 0.0);
@@ -1448,9 +1585,10 @@ namespace armarx
double tau4 = tau3 * tau;
double tau5 = tau4 * tau;
double st = b0 + b1 * tau + b2 * tau2 + b3 * tau3 + b4 * tau4 + b5 * tau5;
- double vt = b1 / D + 2 * b2 / D * tau + 3 * b3 / D * tau2 + 4 * b4 / D * tau3 + 5 * b5 / D * tau4;
+ double vt = b1 / D + 2 * b2 / D * tau + 3 * b3 / D * tau2 + 4 * b4 / D * tau3 +
+ 5 * b5 / D * tau4;
double at = 2 * b2 / D2 + 6 * b3 / D2 * tau + 12 * b4 / D2 * tau2 + 20 * b5 / D2 * tau3;
- return State {t, st, vt, at};
+ return State{t, st, vt, at};
};
double remainingTime = finishTime - currentTime;
@@ -1484,18 +1622,20 @@ namespace armarx
double newJerk = (newAcc - currentAcc) / dt;
double newPos = currentPosition + newVel * dt;
- Output result {newPos, newVel, newAcc, newJerk};
+ Output result{newPos, newVel, newAcc, newJerk};
currentTime += dt;
// State s = min_jerk(currentTime, finishTime, fixedMinJerkState->s0, fixedMinJerkState->v0, fixedMinJerkState->a0, targetPosition, fixedMinJerkState->t0);
// ARMARX_INFO << deactivateSpam(0.1) << VAROUT(remainingTime) << VAROUT(signedDistance) << VAROUT(signedDistance * currentP_Phase3) << VAROUT(currentP_Phase3);// << VAROUT(s.s) << VAROUT(s.v) << VAROUT(s.a);
return result;
}
- if (std::abs(signedDistance) < phase2SwitchDistance && remainingTime < phase2SwitchMaxRemainingTime)
+ if (std::abs(signedDistance) < phase2SwitchDistance &&
+ remainingTime < phase2SwitchMaxRemainingTime)
{
if (!fixedMinJerkState)
{
- fixedMinJerkState.reset(new FixedMinJerkState {currentTime, currentPosition, currentV, currentAcc});
+ fixedMinJerkState.reset(
+ new FixedMinJerkState{currentTime, currentPosition, currentV, currentAcc});
currentP_Phase2 = 0;
}
// std::vector<State> states;
@@ -1507,30 +1647,42 @@ namespace armarx
// states.push_back(s);
// }
currentTime += dt;
- State s = min_jerk(currentTime, finishTime, fixedMinJerkState->s0, fixedMinJerkState->v0, fixedMinJerkState->a0, targetPosition, fixedMinJerkState->t0);
+ State s = min_jerk(currentTime,
+ finishTime,
+ fixedMinJerkState->s0,
+ fixedMinJerkState->v0,
+ fixedMinJerkState->a0,
+ targetPosition,
+ fixedMinJerkState->t0);
double newVelocity = (s.s - (currentPosition + s.v * dt)) * currentP_Phase2 + s.v;
double newAcc = (newVelocity - currentV) / dt;
double newJerk = (newAcc - currentAcc) / dt;
- double newPos = ctrlutil::s(dt, currentPosition, currentV, currentAcc, newJerk);//currentPosition + newVelocity * dt;
+ double newPos = ctrlutil::s(dt,
+ currentPosition,
+ currentV,
+ currentAcc,
+ newJerk); //currentPosition + newVelocity * dt;
// ARMARX_INFO /*<< deactivateSpam(0.1) */ << VAROUT(currentP_Phase2) << VAROUT(currentPosition) << VAROUT(signedDistance) << VAROUT(s.s) << VAROUT(s.v) << VAROUT(newVelocity) << VAROUT(newAcc) << VAROUT(newJerk) << VAROUT(s.a);
currentP_Phase2 = currentP_Phase2 * p_adjust_ratio + p * (1.0 - p_adjust_ratio);
- Output result {newPos, newVelocity, newAcc, newJerk};
+ Output result{newPos, newVelocity, newAcc, newJerk};
return result;
}
-
-
- double jerk = min_jerk_calc_jerk(remainingTime, currentPosition, currentV, currentAcc, targetPosition);
+ double jerk = min_jerk_calc_jerk(
+ remainingTime, currentPosition, currentV, currentAcc, targetPosition);
double newAcc = currentAcc + jerk * dt;
double newVelocity = ctrlutil::v(dt, currentV, currentAcc, jerk);
- double accAtEnd = ctrlutil::a(remainingTime, currentAcc, jerk); // assumes that fixed jerk would be used, which is not the case here
- (void) accAtEnd;
+ double accAtEnd =
+ ctrlutil::a(remainingTime,
+ currentAcc,
+ jerk); // assumes that fixed jerk would be used, which is not the case here
+ (void)accAtEnd;
std::vector<State> states;
std::vector<State> states2;
//#define debug
-#ifdef debug
+#ifdef debug
{
double dt = 0.001;
double t = 1;
@@ -1542,7 +1694,7 @@ namespace armarx
simV = ctrlutil::v(dt, simV, simAcc, 0);
simS += dt * simV;
t -= dt;
- states.push_back(State {t, simS, simV, simAcc, jerk});
+ states.push_back(State{t, simS, simV, simAcc, jerk});
}
}
{
@@ -1557,7 +1709,7 @@ namespace armarx
simV = ctrlutil::v(dt, simV, simAcc, jerk);
simAcc = simAcc + jerk * dt;
t -= dt;
- states2.push_back(State {t, simS, simV, simAcc, jerk});
+ states2.push_back(State{t, simS, simV, simAcc, jerk});
}
}
#endif
@@ -1566,8 +1718,7 @@ namespace armarx
double newPos = ctrlutil::s(dt, currentPosition, currentV, currentAcc, jerk);
- Output result {newPos, newVelocity, newAcc, jerk};
+ Output result{newPos, newVelocity, newAcc, jerk};
return result;
-
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.h b/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
index c544ea8dd4b54eb15cebd9fb10e36a00e0623234..0c35892237fa15de457dca9b2582832c0a40df3e 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
@@ -24,29 +24,32 @@
#include <cmath>
#include <type_traits>
+
#include <ArmarXCore/core/util/algorithm.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
+
#include <RobotAPI/libraries/core/PIDController.h>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
// #define DEBUG_POS_CTRL
#ifdef DEBUG_POS_CTRL
#include <boost/circular_buffer.hpp>
#endif
-
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)
+ 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)
+ 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 a = -p / 2;
T b = std::sqrt(std::pow(p / 2, 2) - q);
return {a + b, a - b};
}
@@ -58,7 +61,8 @@ namespace armarx
float dt;
};
- inline deltas accelerateToVelocity(float v0, float acc, float vt)
+ inline deltas
+ accelerateToVelocity(float v0, float acc, float vt)
{
acc = std::abs(acc);
deltas d;
@@ -70,21 +74,29 @@ namespace armarx
std::array<deltas, 3> trapeze(float v0, float acc, float vMax, float dec, float vt, float dx);
-
- inline float trapezeArea(float v0, float vmax, float dt1, float dt2, float dt3)
+ inline float
+ trapezeArea(float v0, float vmax, float dt1, float dt2, float dt3)
{
return (v0 + vmax) / 2 * dt1 + dt2 * vmax + dt3 * vmax * 0.5;
}
-
- inline void findVelocityAndAccelerationForTimeAndDistance(float distance, float v0, float vmax, float dec, std::array<deltas, 3> trapeze, float newDt, float& newV, float& newAcc, float& newDec)
+ inline void
+ findVelocityAndAccelerationForTimeAndDistance(float distance,
+ float v0,
+ float vmax,
+ float dec,
+ std::array<deltas, 3> trapeze,
+ float newDt,
+ float& newV,
+ float& newAcc,
+ float& newDec)
{
float oldDt = trapeze.at(0).dt + trapeze.at(1).dt + trapeze.at(2).dt;
- float dt0 = trapeze.at(0).dt;
- float dt1 = trapeze.at(1).dt;
- float dt2 = trapeze.at(2).dt;
+ float dt0 = trapeze.at(0).dt;
+ float dt1 = trapeze.at(1).dt;
+ float dt2 = trapeze.at(2).dt;
float area = trapezeArea(v0, vmax, dt0, dt1, dt2);
dt1 += newDt - oldDt;
newV = (-dt0 * v0 + 2 * area) / (dt0 + 2 * dt1 + dt2);
@@ -130,25 +142,25 @@ namespace armarx
// newAcc = dec;
// newDec = dec;
-
}
-
-
}
- std::array<deltas, 3> trapezeWithDt(float v0, float acc, float vMax, float dec, float vt, float dx, float dt);
+ 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)
+ inline float
+ brakingDistance(float v0, float deceleration)
{
return accelerateToVelocity(std::abs(v0), std::abs(deceleration), 0).dx;
}
- inline float angleDistance(float angle1, float angle2)
+ inline float
+ angleDistance(float angle1, float angle2)
{
return M_PI - std::fabs(std::fmod(std::fabs(angle1 - angle2), M_PI * 2) - M_PI);
}
@@ -178,6 +190,7 @@ namespace armarx
double acceleration;
double jerk;
};
+
// config
float maxDt;
float maxV;
@@ -195,8 +208,10 @@ namespace armarx
Output run() const;
};
- typedef std::shared_ptr<class RampedAccelerationVelocityControllerConfiguration> RampedAccelerationVelocityControllerConfigurationPtr;
- typedef std::shared_ptr<const RampedAccelerationVelocityControllerConfiguration> RampedAccelerationVelocityControllerConfigurationCPtr;
+ typedef std::shared_ptr<class RampedAccelerationVelocityControllerConfiguration>
+ RampedAccelerationVelocityControllerConfigurationPtr;
+ typedef std::shared_ptr<const RampedAccelerationVelocityControllerConfiguration>
+ RampedAccelerationVelocityControllerConfigurationCPtr;
class RampedAccelerationVelocityControllerConfiguration
{
@@ -210,7 +225,8 @@ namespace armarx
float directSetVLimit;
};
- struct VelocityControllerWithRampedAccelerationAndPositionBounds: VelocityControllerWithRampedAcceleration
+ struct VelocityControllerWithRampedAccelerationAndPositionBounds :
+ VelocityControllerWithRampedAcceleration
{
double currentPosition;
@@ -223,11 +239,14 @@ namespace armarx
Output run() const;
};
- float velocityControlWithAccelerationBounds(
- float dt, float maxDt,
- const float currentV, float targetV, float maxV,
- float acceleration, float deceleration,
- float directSetVLimit);
+ 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.
@@ -250,7 +269,8 @@ namespace armarx
* @param positionLimitHi
* @return The next velocity.
*/
- struct VelocityControllerWithAccelerationAndPositionBounds: VelocityControllerWithAccelerationBounds
+ struct VelocityControllerWithAccelerationAndPositionBounds :
+ VelocityControllerWithAccelerationBounds
{
float currentPosition;
@@ -263,16 +283,19 @@ namespace armarx
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);
-
+ 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
{
@@ -285,7 +308,8 @@ namespace armarx
float currentPosition;
float targetPosition;
float pControlPosErrorLimit = 0.01;
- float pControlVelLimit = 0.002; // if target is below this threshold, PID controller will be used
+ float pControlVelLimit =
+ 0.002; // if target is below this threshold, PID controller will be used
float accuracy = 0.001;
std::shared_ptr<PIDController> pid;
// float p;
@@ -296,6 +320,7 @@ namespace armarx
float estimateTime() const;
#ifdef DEBUG_POS_CTRL
mutable bool PIDModeActive = false;
+
struct HelpStruct
{
float currentPosition;
@@ -315,7 +340,6 @@ namespace armarx
private:
mutable bool currentlyPIDActive = false;
-
};
struct PositionThroughVelocityControllerWithAccelerationRamps
@@ -329,8 +353,8 @@ namespace armarx
DecAccIncJerk,
DecAccDecJerk,
PCtrl
-
};
+
struct Output
{
double velocity;
@@ -347,14 +371,17 @@ namespace armarx
double deceleration;
double jerk;
double currentPosition;
+
protected:
double targetPosition;
+
public:
double getTargetPosition() const;
void setTargetPosition(double value);
double pControlPosErrorLimit = 0.01;
- double pControlVelLimit = 0.002; // if target is below this threshold, PID controller will be used
+ double pControlVelLimit =
+ 0.002; // if target is below this threshold, PID controller will be used
double accuracy = 0.001;
double p;
bool usePIDAtEnd = true;
@@ -369,6 +396,7 @@ namespace armarx
std::pair<State, Output> calcState() const;
#ifdef DEBUG_POS_CTRL
mutable bool PIDModeActive = false;
+
struct HelpStruct
{
double currentPosition;
@@ -382,16 +410,20 @@ namespace armarx
mutable boost::circular_buffer<HelpStruct> buffer;
mutable int eventHappeningCounter = -1;
#endif
-
};
- float positionThroughVelocityControlWithAccelerationBounds(float dt, float maxDt,
- float currentV, float maxV,
- float acceleration, float deceleration,
- float currentPosition, float targetPosition,
- float p);
-
- struct PositionThroughVelocityControllerWithAccelerationAndPositionBounds: PositionThroughVelocityControllerWithAccelerationBounds
+ 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;
@@ -399,26 +431,42 @@ namespace armarx
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 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);
+
+ 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);
class MinJerkPositionController
{
@@ -427,10 +475,12 @@ namespace armarx
{
double t, s, v, a;
};
+
struct FixedMinJerkState
{
double t0, s0, v0, a0;
};
+
struct Output
{
double position;
@@ -459,6 +509,7 @@ namespace armarx
double finishTime = 0;
double currentTime = 0;
std::unique_ptr<FixedMinJerkState> fixedMinJerkState;
+
public:
void reset();
double getTargetPosition() const;
@@ -466,5 +517,4 @@ namespace armarx
Output run();
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Constants.h b/source/RobotAPI/components/units/RobotUnit/Constants.h
index b8452dcd42700f5313a96366844c019a55781c38..30680e31423c8365d64a8ab1544ba7f55e4ca3d9 100644
--- a/source/RobotAPI/components/units/RobotUnit/Constants.h
+++ b/source/RobotAPI/components/units/RobotUnit/Constants.h
@@ -31,5 +31,4 @@ namespace armarx::ControllerConstants
#pragma GCC diagnostic ignored "-Wunused-variable"
static const float ValueNotSetNaN = std::nanf(std::to_string((1 << 16) - 1).c_str());
#pragma GCC diagnostic pop
-}
-
+} // namespace armarx::ControllerConstants
diff --git a/source/RobotAPI/components/units/RobotUnit/ControlModes.h b/source/RobotAPI/components/units/RobotUnit/ControlModes.h
index b8b5d4e7cdec1eb2fcfbea1902bf25ccf4d2ea6a..9309cbc1155b8ad7f3080d73a3a67952f0713019 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControlModes.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlModes.h
@@ -28,8 +28,8 @@ namespace armarx::ControlModes
{
//'normal' actor modes
static const std::string Position1DoF = "ControlMode_Position1DoF";
- static const std::string Torque1DoF = "ControlMode_Torque1DoF";
- static const std::string ZeroTorque1DoF = "ControlMode_ZeroTorque1DoF";
+ static const std::string Torque1DoF = "ControlMode_Torque1DoF";
+ static const std::string ZeroTorque1DoF = "ControlMode_ZeroTorque1DoF";
static const std::string Velocity1DoF = "ControlMode_Velocity1DoF";
static const std::string Current1DoF = "ControlMode_Current1DoF";
static const std::string PWM1DoF = "ControlMode_PWM1DoF";
@@ -43,12 +43,13 @@ namespace armarx::ControlModes
//modes for holonomic platforms
static const std::string HolonomicPlatformVelocity = "ControlMode_HolonomicPlatformVelocity";
- static const std::string HolonomicPlatformRelativePosition = "ControlMode_HolonomicPlatformRelativePosition";
+ static const std::string HolonomicPlatformRelativePosition =
+ "ControlMode_HolonomicPlatformRelativePosition";
//special sentinel modes
static const std::string EmergencyStop = "ControlMode_EmergencyStop";
static const std::string StopMovement = "ControlMode_StopMovement";
-}
+} // namespace armarx::ControlModes
namespace armarx::HardwareControlModes
{
@@ -59,4 +60,4 @@ namespace armarx::HardwareControlModes
//modes for holonomic platforms
static const std::string HolonomicPlatformVelocity = "HolonomicPlatformVelocity";
-}
+} // namespace armarx::HardwareControlModes
diff --git a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h
index 68512914578343ac958a7ed4377b427a04357603..460aed21bc55edda36a0c3f132ff74eb1621beff 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h
@@ -22,102 +22,147 @@
#pragma once
-#include "ControlTargetBase.h"
-
#include <RobotAPI/components/units/RobotUnit/Constants.h> // for ControllerConstants::ValueNotSetNaN
+#include "ControlTargetBase.h"
+
namespace armarx
{
-#define make_ControlTarget1DoFActuator(type, invalid, name, varname, cmode) \
- class name: public ControlTargetBase \
- { \
- public: \
- type varname = ControllerConstants::ValueNotSetNaN; \
- name() = default; \
- name(const name&) = default; \
- name(name&&) = default; \
- name(type val) : varname{val} {} \
- name& operator=(const name&) = default; \
- name& operator=(name&&) = default; \
- name& operator=(type val) {varname = val; return *this;} \
- virtual const std::string& getControlMode() const override \
- { \
- return cmode; \
- } \
- virtual void reset() override \
- { \
- varname = invalid; \
- } \
- virtual bool isValid() const override \
- { \
- return std::isfinite(varname); \
- } \
- DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION \
- static ControlTargetInfo<name> GetClassMemberInfo() \
- { \
- ControlTargetInfo<name> cti; \
- cti.addMemberVariable(&name::varname, #varname); \
- return cti; \
- } \
+#define make_ControlTarget1DoFActuator(type, invalid, name, varname, cmode) \
+ class name : public ControlTargetBase \
+ { \
+ public: \
+ type varname = ControllerConstants::ValueNotSetNaN; \
+ name() = default; \
+ name(const name&) = default; \
+ name(name&&) = default; \
+ name(type val) : varname{val} \
+ { \
+ } \
+ name& operator=(const name&) = default; \
+ name& operator=(name&&) = default; \
+ name& \
+ operator=(type val) \
+ { \
+ varname = val; \
+ return *this; \
+ } \
+ virtual const std::string& \
+ getControlMode() const override \
+ { \
+ return cmode; \
+ } \
+ virtual void \
+ reset() override \
+ { \
+ varname = invalid; \
+ } \
+ virtual bool \
+ isValid() const override \
+ { \
+ return std::isfinite(varname); \
+ } \
+ DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION static ControlTargetInfo<name> \
+ GetClassMemberInfo() \
+ { \
+ ControlTargetInfo<name> cti; \
+ cti.addMemberVariable(&name::varname, #varname); \
+ return cti; \
+ } \
}
- make_ControlTarget1DoFActuator(float, ControllerConstants::ValueNotSetNaN, ControlTarget1DoFActuatorPosition, position, ControlModes::Position1DoF);
- make_ControlTarget1DoFActuator(float, ControllerConstants::ValueNotSetNaN, ControlTarget1DoFActuatorVelocity, velocity, ControlModes::Velocity1DoF);
- make_ControlTarget1DoFActuator(float, ControllerConstants::ValueNotSetNaN, ControlTarget1DoFActuatorTorque, torque, ControlModes::Torque1DoF);
- make_ControlTarget1DoFActuator(float, ControllerConstants::ValueNotSetNaN, ControlTarget1DoFActuatorZeroTorque, torque, ControlModes::ZeroTorque1DoF);
- make_ControlTarget1DoFActuator(float, ControllerConstants::ValueNotSetNaN, ControlTarget1DoFActuatorCurrent, current, ControlModes::Current1DoF);
+ make_ControlTarget1DoFActuator(float,
+ ControllerConstants::ValueNotSetNaN,
+ ControlTarget1DoFActuatorPosition,
+ position,
+ ControlModes::Position1DoF);
+ make_ControlTarget1DoFActuator(float,
+ ControllerConstants::ValueNotSetNaN,
+ ControlTarget1DoFActuatorVelocity,
+ velocity,
+ ControlModes::Velocity1DoF);
+ make_ControlTarget1DoFActuator(float,
+ ControllerConstants::ValueNotSetNaN,
+ ControlTarget1DoFActuatorTorque,
+ torque,
+ ControlModes::Torque1DoF);
+ make_ControlTarget1DoFActuator(float,
+ ControllerConstants::ValueNotSetNaN,
+ ControlTarget1DoFActuatorZeroTorque,
+ torque,
+ ControlModes::ZeroTorque1DoF);
+ make_ControlTarget1DoFActuator(float,
+ ControllerConstants::ValueNotSetNaN,
+ ControlTarget1DoFActuatorCurrent,
+ current,
+ ControlModes::Current1DoF);
#undef make_ControlTarget1DoFActuator
- class ControlTarget1DoFActuatorTorqueVelocity: public ControlTarget1DoFActuatorVelocity
+ class ControlTarget1DoFActuatorTorqueVelocity : public ControlTarget1DoFActuatorVelocity
{
public:
float maxTorque;
- const std::string& getControlMode() const override
+ const std::string&
+ getControlMode() const override
{
return ControlModes::VelocityTorque;
}
- void reset() override
+
+ void
+ reset() override
{
ControlTarget1DoFActuatorVelocity::reset();
maxTorque = -1.0f; // if < 0, default value for joint is to be used
}
- bool isValid() const override
+
+ bool
+ isValid() const override
{
return ControlTarget1DoFActuatorVelocity::isValid() && std::isfinite(maxTorque);
}
- static ControlTargetInfo<ControlTarget1DoFActuatorTorqueVelocity> GetClassMemberInfo()
+
+ static ControlTargetInfo<ControlTarget1DoFActuatorTorqueVelocity>
+ GetClassMemberInfo()
{
ControlTargetInfo<ControlTarget1DoFActuatorTorqueVelocity> cti;
cti.addBaseClass<ControlTarget1DoFActuatorVelocity>();
cti.addMemberVariable(&ControlTarget1DoFActuatorTorqueVelocity::maxTorque, "maxTorque");
return cti;
}
+
DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
};
- class ActiveImpedanceControlTarget: public ControlTargetBase
+ class ActiveImpedanceControlTarget : public ControlTargetBase
{
public:
float position;
float kp;
float kd;
- const std::string& getControlMode() const override
+ const std::string&
+ getControlMode() const override
{
return ControlModes::ActiveImpedance;
}
- void reset() override
+
+ void
+ reset() override
{
position = 0;
kp = 0;
kd = 0;
}
- bool isValid() const override
+
+ bool
+ isValid() const override
{
return std::isfinite(position) && kp >= 0;
}
- static ControlTargetInfo<ActiveImpedanceControlTarget> GetClassMemberInfo()
+
+ static ControlTargetInfo<ActiveImpedanceControlTarget>
+ GetClassMemberInfo()
{
ControlTargetInfo<ActiveImpedanceControlTarget> cti;
cti.addMemberVariable(&ActiveImpedanceControlTarget::position, "position");
@@ -125,6 +170,7 @@ namespace armarx
cti.addMemberVariable(&ActiveImpedanceControlTarget::kd, "kd");
return cti;
}
+
DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
};
@@ -132,101 +178,134 @@ namespace armarx
{
public:
std::int16_t pwm;
+
public:
- const std::string& getControlMode() const override
+ const std::string&
+ getControlMode() const override
{
return ControlModes::PWM1DoF;
}
- void reset() override
+
+ void
+ reset() override
{
pwm = std::numeric_limits<std::int16_t>::max();
}
- bool isValid() const override
+
+ bool
+ isValid() const override
{
- return std::abs(pwm) < std::numeric_limits<std::int16_t>::max() ;
+ return std::abs(pwm) < std::numeric_limits<std::int16_t>::max();
}
- static ControlTargetInfo<ControlTarget1DoFActuatorPWM> GetClassMemberInfo()
+
+ static ControlTargetInfo<ControlTarget1DoFActuatorPWM>
+ GetClassMemberInfo()
{
ControlTargetInfo<ControlTarget1DoFActuatorPWM> cti;
cti.addMemberVariable(&ControlTarget1DoFActuatorPWM::pwm, "pwm_motor");
return cti;
}
+
DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
};
- class ControlTarget1DoFActuatorPositionWithPWMLimit: public ControlTarget1DoFActuatorPosition
+ class ControlTarget1DoFActuatorPositionWithPWMLimit : public ControlTarget1DoFActuatorPosition
{
public:
int16_t maxPWM;
+
protected:
std::int16_t pwmDefaultLimit = 256;
std::int16_t pwmHardLimit = 256;
+
public:
- const std::string& getControlMode() const override
+ const std::string&
+ getControlMode() const override
{
return ControlModes::PositionWithPwmLimit1DoF;
}
- void reset() override
+
+ void
+ reset() override
{
maxPWM = pwmDefaultLimit;
ControlTarget1DoFActuatorPosition::reset();
}
- bool isValid() const override
+
+ bool
+ isValid() const override
{
- return std::abs(maxPWM) <= pwmHardLimit &&
- ControlTarget1DoFActuatorPosition::isValid();
+ return std::abs(maxPWM) <= pwmHardLimit && ControlTarget1DoFActuatorPosition::isValid();
}
- static ControlTargetInfo<ControlTarget1DoFActuatorPositionWithPWMLimit> GetClassMemberInfo()
+
+ static ControlTargetInfo<ControlTarget1DoFActuatorPositionWithPWMLimit>
+ GetClassMemberInfo()
{
ControlTargetInfo<ControlTarget1DoFActuatorPositionWithPWMLimit> cti;
cti.addBaseClass<ControlTarget1DoFActuatorPosition>();
cti.addMemberVariable(&ControlTarget1DoFActuatorPositionWithPWMLimit::maxPWM, "maxPWM");
- cti.addMemberVariable(&ControlTarget1DoFActuatorPositionWithPWMLimit::pwmHardLimit, "pwmHardLimit");
+ cti.addMemberVariable(&ControlTarget1DoFActuatorPositionWithPWMLimit::pwmHardLimit,
+ "pwmHardLimit");
return cti;
}
- void setPWMLimits(std::int16_t hard, std::int16_t def, ControlDeviceAccessToken)
+
+ void
+ setPWMLimits(std::int16_t hard, std::int16_t def, ControlDeviceAccessToken)
{
pwmHardLimit = hard;
pwmDefaultLimit = def;
}
+
DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
};
- class ControlTarget1DoFActuatorVelocityWithPWMLimit: public ControlTarget1DoFActuatorVelocity
+
+ class ControlTarget1DoFActuatorVelocityWithPWMLimit : public ControlTarget1DoFActuatorVelocity
{
public:
int16_t maxPWM;
+
protected:
std::int16_t pwmDefaultLimit = 256;
std::int16_t pwmHardLimit = 256;
+
public:
- const std::string& getControlMode() const override
+ const std::string&
+ getControlMode() const override
{
return ControlModes::VelocityWithPwmLimit1DoF;
}
- void reset() override
+
+ void
+ reset() override
{
maxPWM = pwmDefaultLimit;
ControlTarget1DoFActuatorVelocity::reset();
}
- bool isValid() const override
+
+ bool
+ isValid() const override
{
- return std::abs(maxPWM) <= pwmHardLimit &&
- ControlTarget1DoFActuatorVelocity::isValid();
+ return std::abs(maxPWM) <= pwmHardLimit && ControlTarget1DoFActuatorVelocity::isValid();
}
- static ControlTargetInfo<ControlTarget1DoFActuatorVelocityWithPWMLimit> GetClassMemberInfo()
+
+ static ControlTargetInfo<ControlTarget1DoFActuatorVelocityWithPWMLimit>
+ GetClassMemberInfo()
{
ControlTargetInfo<ControlTarget1DoFActuatorVelocityWithPWMLimit> cti;
cti.addBaseClass<ControlTarget1DoFActuatorVelocity>();
cti.addMemberVariable(&ControlTarget1DoFActuatorVelocityWithPWMLimit::maxPWM, "maxPWM");
- cti.addMemberVariable(&ControlTarget1DoFActuatorVelocityWithPWMLimit::pwmHardLimit, "pwmHardLimit");
+ cti.addMemberVariable(&ControlTarget1DoFActuatorVelocityWithPWMLimit::pwmHardLimit,
+ "pwmHardLimit");
return cti;
}
- void setPWMLimits(std::int16_t hard, std::int16_t def, ControlDeviceAccessToken)
+
+ void
+ setPWMLimits(std::int16_t hard, std::int16_t def, ControlDeviceAccessToken)
{
pwmHardLimit = hard;
pwmDefaultLimit = def;
}
+
DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
index cf19d2b9f4cf9583bbd5e75e82b71f7cca6e8d0d..ca3b048750edfaa65e0737f9d7d2ec4bda740821 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
@@ -22,17 +22,16 @@
#pragma once
-#include <RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h>
-#include <RobotAPI/components/units/RobotUnit//util/HeterogenousContinuousContainerMacros.h>
-#include <RobotAPI/components/units/RobotUnit/ControlModes.h>
-
-#include <IceUtil/Time.h>
-#include <Ice/Handle.h>
-
+#include <map>
#include <memory>
#include <string>
-#include <map>
+#include <Ice/Handle.h>
+#include <IceUtil/Time.h>
+
+#include <RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainerMacros.h>
+#include <RobotAPI/components/units/RobotUnit/ControlModes.h>
+#include <RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h>
namespace armarx
{
@@ -55,7 +54,7 @@ namespace armarx
ControlDeviceAccessToken() = default;
};
- template<class DerivedClass>
+ template <class DerivedClass>
using ControlTargetInfo = introspection::ClassMemberInfo<ControlTargetBase, DerivedClass>;
virtual ~ControlTargetBase() = default;
@@ -65,138 +64,152 @@ namespace armarx
virtual void reset() = 0;
virtual bool isValid() const = 0;
- template<class T>
- bool isA() const
+ template <class T>
+ bool
+ isA() const
{
return asA<T>();
}
- template<class T>
- const T* asA() const
+ template <class T>
+ const T*
+ asA() const
{
return dynamic_cast<const T*>(this);
}
- template<class T>
- T* asA()
+ template <class T>
+ T*
+ asA()
{
return dynamic_cast<T*>(this);
}
//logging functions
/// @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 = 0;
+ virtual std::map<std::string, VariantBasePtr>
+ toVariants(const IceUtil::Time& timestamp) const = 0;
virtual std::size_t getNumberOfDataFields() const = 0;
virtual std::vector<std::string> getDataFieldNames() const = 0;
- virtual void getDataFieldAs(std::size_t i, bool& out) const = 0;
- virtual void getDataFieldAs(std::size_t i, Ice::Byte& out) const = 0;
- virtual void getDataFieldAs(std::size_t i, Ice::Short& out) const = 0;
- virtual void getDataFieldAs(std::size_t i, Ice::Int& out) const = 0;
- virtual void getDataFieldAs(std::size_t i, Ice::Long& out) const = 0;
- virtual void getDataFieldAs(std::size_t i, Ice::Float& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, bool& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Byte& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Short& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Int& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Long& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Float& out) const = 0;
virtual void getDataFieldAs(std::size_t i, Ice::Double& out) const = 0;
virtual void getDataFieldAs(std::size_t i, std::string& out) const = 0;
virtual const std::type_info& getDataFieldType(std::size_t i) const = 0;
//management functions
- template<class T, class = typename std::enable_if<std::is_base_of<ControlTargetBase, T>::value>::type>
- void _copyTo(std::unique_ptr<T>& target) const
+ 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());
}
- ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(
- ControlTargetHasGetClassMemberInfo,
- GetClassMemberInfo, ControlTargetInfo<T>(*)(void));
+ ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(ControlTargetHasGetClassMemberInfo,
+ GetClassMemberInfo,
+ ControlTargetInfo<T> (*)(void));
ARMARX_PLACEMENT_CONSTRUCTION_HELPER_BASE(ControlTargetBase)
};
-#define DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION \
- ARMARX_PLACEMENT_CONSTRUCTION_HELPER \
- using ControlTargetBase = ::armarx::ControlTargetBase; \
- using VariantBasePtr = ::armarx::VariantBasePtr; \
- std::string getControlTargetType(bool withoutNamespaceSpecifier = false) const override \
- { \
- return armarx::GetTypeString(*this, withoutNamespaceSpecifier); \
- } \
- void _check_for_static_GetClassMemberInfo_overload() \
- { \
- static_assert(ControlTargetHasGetClassMemberInfo<std::decay<decltype(*this)>::type>::value, \
- "This class has to implement GetClassMemberInfo() returning " \
- "an instance of ControlTargetInfo<THIS_CLASS_TYPE>"); \
- } \
- std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override \
- { \
- return ControlTargetInfo<std::decay<decltype(*this)>::type>::ToVariants(timestamp,this); \
- } \
- std::size_t getNumberOfDataFields() const override \
- { \
- return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetNumberOfDataFields(); \
- } \
- void getDataFieldAs (std::size_t i, bool& out) const override \
- { \
- ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Byte& out) const override \
- { \
- ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Short& out) const override \
- { \
- ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Int& out) const override \
- { \
- ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Long& out) const override \
- { \
- ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Float& out) const override \
- { \
- ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Double& out) const override \
- { \
- ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, std::string& out) const override \
- { \
- ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- const std::type_info& getDataFieldType(std::size_t i) const override \
- { \
- return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldType(i); \
- } \
- std::vector<std::string> getDataFieldNames() const override \
- { \
- return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldNames(); \
+#define DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION \
+ ARMARX_PLACEMENT_CONSTRUCTION_HELPER \
+ using ControlTargetBase = ::armarx::ControlTargetBase; \
+ using VariantBasePtr = ::armarx::VariantBasePtr; \
+ std::string getControlTargetType(bool withoutNamespaceSpecifier = false) const override \
+ { \
+ return armarx::GetTypeString(*this, withoutNamespaceSpecifier); \
+ } \
+ void _check_for_static_GetClassMemberInfo_overload() \
+ { \
+ static_assert( \
+ ControlTargetHasGetClassMemberInfo<std::decay<decltype(*this)>::type>::value, \
+ "This class has to implement GetClassMemberInfo() returning " \
+ "an instance of ControlTargetInfo<THIS_CLASS_TYPE>"); \
+ } \
+ std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) \
+ const override \
+ { \
+ return ControlTargetInfo<std::decay<decltype(*this)>::type>::ToVariants(timestamp, this); \
+ } \
+ std::size_t getNumberOfDataFields() const override \
+ { \
+ return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetNumberOfDataFields(); \
+ } \
+ void getDataFieldAs(std::size_t i, bool& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Byte& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Short& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Int& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Long& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Float& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Double& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, std::string& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ const std::type_info& getDataFieldType(std::size_t i) const override \
+ { \
+ return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldType(i); \
+ } \
+ std::vector<std::string> getDataFieldNames() const override \
+ { \
+ return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldNames(); \
}
-#define make_DummyControlTarget(Suffix,ControlMode) \
- class DummyControlTarget##Suffix : public ControlTargetBase \
- { \
- public: \
- virtual const std::string& getControlMode() const override \
- { \
- return ControlMode; \
- } \
- virtual void reset() override {} \
- virtual bool isValid() const override \
- { \
- return true; \
- } \
- DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION \
- static ControlTargetInfo<DummyControlTarget##Suffix> \
- GetClassMemberInfo() \
- { \
- return ControlTargetInfo<DummyControlTarget##Suffix> {}; \
- } \
+#define make_DummyControlTarget(Suffix, ControlMode) \
+ class DummyControlTarget##Suffix : public ControlTargetBase \
+ { \
+ public: \
+ virtual const std::string& \
+ getControlMode() const override \
+ { \
+ return ControlMode; \
+ } \
+ virtual void \
+ reset() override \
+ { \
+ } \
+ virtual bool \
+ isValid() const override \
+ { \
+ return true; \
+ } \
+ DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION static ControlTargetInfo< \
+ DummyControlTarget##Suffix> \
+ GetClassMemberInfo() \
+ { \
+ return ControlTargetInfo<DummyControlTarget##Suffix>{}; \
+ } \
}
make_DummyControlTarget(EmergencyStop, ControlModes::EmergencyStop);
make_DummyControlTarget(StopMovement, ControlModes::StopMovement);
#undef make_DummyControlTarget
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h
index cad312b44637034c4c254a8b300e00c42cbb09fb..dc2fc32863abf6e9384cebd9188968dfd419c9e2 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h
@@ -22,10 +22,10 @@
#pragma once
-#include "ControlTargetBase.h"
-
#include <RobotAPI/components/units/RobotUnit/Constants.h> // for ControllerConstants::ValueNotSetNaN
+#include "ControlTargetBase.h"
+
namespace armarx
{
/**
@@ -35,36 +35,44 @@ namespace armarx
*
* Detailed description of class ControlTargetHolonomicPlatformVelocity.
*/
- class ControlTargetHolonomicPlatformVelocity: public ControlTargetBase
+ class ControlTargetHolonomicPlatformVelocity : public ControlTargetBase
{
public:
float velocityX = ControllerConstants::ValueNotSetNaN;
float velocityY = ControllerConstants::ValueNotSetNaN;
float velocityRotation = ControllerConstants::ValueNotSetNaN;
- const std::string& getControlMode() const override
+
+ const std::string&
+ getControlMode() const override
{
return ControlModes::HolonomicPlatformVelocity;
}
- void reset() override
+
+ void
+ reset() override
{
velocityX = ControllerConstants::ValueNotSetNaN;
velocityY = ControllerConstants::ValueNotSetNaN;
velocityRotation = ControllerConstants::ValueNotSetNaN;
}
- bool isValid() const override
+
+ bool
+ isValid() const override
{
- return std::isfinite(velocityX) && std::isfinite(velocityY) && std::isfinite(velocityRotation);
+ return std::isfinite(velocityX) && std::isfinite(velocityY) &&
+ std::isfinite(velocityRotation);
}
- DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
- static ControlTargetInfo<ControlTargetHolonomicPlatformVelocity> GetClassMemberInfo()
+ DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION static ControlTargetInfo<
+ ControlTargetHolonomicPlatformVelocity>
+ GetClassMemberInfo()
{
ControlTargetInfo<ControlTargetHolonomicPlatformVelocity> cti;
cti.addMemberVariable(&ControlTargetHolonomicPlatformVelocity::velocityX, "velocityX");
cti.addMemberVariable(&ControlTargetHolonomicPlatformVelocity::velocityY, "velocityY");
- cti.addMemberVariable(&ControlTargetHolonomicPlatformVelocity::velocityRotation, "velocityRotation");
+ cti.addMemberVariable(&ControlTargetHolonomicPlatformVelocity::velocityRotation,
+ "velocityRotation");
return cti;
}
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.cpp b/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.cpp
index a2cce575f38d3bb3f015fd3decdbcb30d3d816bb..5ea5de5a0decff5792d66ea890a1e39729c5c4a0 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.cpp
@@ -1,30 +1,30 @@
#include "CartesianImpedanceController.h"
+#include <algorithm>
+
+#include <SimoxUtility/math/compare/is_equal.h>
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
-#include <SimoxUtility/math/compare/is_equal.h>
-
#include <VirtualRobot/MathTools.h>
-#include <algorithm>
-
using namespace armarx;
-
-int CartesianImpedanceController::bufferSize() const
+int
+CartesianImpedanceController::bufferSize() const
{
return targetJointTorques.size();
}
-void CartesianImpedanceController::initialize(const VirtualRobot::RobotNodeSetPtr& rns)
+
+void
+CartesianImpedanceController::initialize(const VirtualRobot::RobotNodeSetPtr& rns)
{
ARMARX_CHECK_NOT_NULL(rns);
tcp = rns->getTCP();
ARMARX_CHECK_NOT_NULL(tcp);
ik.reset(new VirtualRobot::DifferentialIK(
- rns, rns->getRobot()->getRootNode(),
- VirtualRobot::JacobiProvider::eSVDDamped));
+ rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
const auto size = rns->getSize();
qpos.resize(size);
@@ -36,14 +36,16 @@ void CartesianImpedanceController::initialize(const VirtualRobot::RobotNodeSetPt
I = Eigen::MatrixXf::Identity(size, size);
}
-const Eigen::VectorXf& CartesianImpedanceController::run(
+
+const Eigen::VectorXf&
+CartesianImpedanceController::run(
const Config& cfg,
std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors,
std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors,
- std::vector<const SensorValue1DoFActuatorPosition*> positionSensors
-)
+ std::vector<const SensorValue1DoFActuatorPosition*> positionSensors)
{
- ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), cfg.Knull.size())); ///TODO move this to core
+ ARMARX_CHECK_EXPRESSION(
+ simox::math::is_equal(bufferSize(), cfg.Knull.size())); ///TODO move this to core
ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), cfg.Dnull.size()));
ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), cfg.desiredJointPosition.size()));
ARMARX_CHECK_GREATER_EQUAL(cfg.torqueLimit, 0);
@@ -52,10 +54,12 @@ const Eigen::VectorXf& CartesianImpedanceController::run(
ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), velocitySensors.size()));
ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), positionSensors.size()));
- const Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All); ///TODO output param version
+ const Eigen::MatrixXf jacobi = ik->getJacobianMatrix(
+ tcp, VirtualRobot::IKSolver::CartesianSelection::All); ///TODO output param version
ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), jacobi.cols()));
ARMARX_CHECK_EXPRESSION(simox::math::is_equal(6, jacobi.rows()));
- const Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), lambda);///TODO output param version
+ const Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(
+ jacobi.transpose(), lambda); ///TODO output param version
for (size_t i = 0; i < velocitySensors.size(); ++i)
{
@@ -65,22 +69,15 @@ const Eigen::VectorXf& CartesianImpedanceController::run(
const Eigen::Vector6f tcptwist = jacobi * qvel;
- const Eigen::Vector3f currentTCPLinearVelocity
- {
- 0.001f * tcptwist(0),
- 0.001f * tcptwist(1),
- 0.001f * tcptwist(2)
- };
- const Eigen::Vector3f currentTCPAngularVelocity
- {
- tcptwist(3),
- tcptwist(4),
- tcptwist(5)
- };
+ const Eigen::Vector3f currentTCPLinearVelocity{
+ 0.001f * tcptwist(0), 0.001f * tcptwist(1), 0.001f * tcptwist(2)};
+ const Eigen::Vector3f currentTCPAngularVelocity{tcptwist(3), tcptwist(4), tcptwist(5)};
const Eigen::Vector3f currentTCPPosition = tcp->getPositionInRootFrame();
- const Eigen::Vector3f tcpForces = 0.001 * cfg.Kpos.cwiseProduct(cfg.desiredPosition - currentTCPPosition);
- const Eigen::Vector3f tcpDesiredForce = tcpForces - cfg.Dpos.cwiseProduct(currentTCPLinearVelocity);
+ const Eigen::Vector3f tcpForces =
+ 0.001 * cfg.Kpos.cwiseProduct(cfg.desiredPosition - currentTCPPosition);
+ const Eigen::Vector3f tcpDesiredForce =
+ tcpForces - cfg.Dpos.cwiseProduct(currentTCPLinearVelocity);
const Eigen::Matrix4f currentTCPPose = tcp->getPoseInRootFrame();
const Eigen::Matrix3f currentRotMat = currentTCPPose.block<3, 3>(0, 0);
@@ -92,10 +89,9 @@ const Eigen::VectorXf& CartesianImpedanceController::run(
const Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
const Eigen::Vector3f tcpDesiredTorque =
- cfg.Kori.cwiseProduct(rpy) -
- cfg.Dori.cwiseProduct(currentTCPAngularVelocity);
+ cfg.Kori.cwiseProduct(rpy) - cfg.Dori.cwiseProduct(currentTCPAngularVelocity);
Eigen::Vector6f tcpDesiredWrench;
- tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
+ tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
// calculate desired joint torque
nullqerror = cfg.desiredJointPosition - qpos;
@@ -110,11 +106,11 @@ const Eigen::VectorXf& CartesianImpedanceController::run(
nullspaceTorque = cfg.Knull.cwiseProduct(nullqerror) - cfg.Dnull.cwiseProduct(qvel);
targetJointTorques =
- jacobi.transpose() * tcpDesiredWrench +
- (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
+ jacobi.transpose() * tcpDesiredWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
for (int i = 0; i < targetJointTorques.size(); ++i)
{
- targetJointTorques(i) = std::clamp(targetJointTorques(i), -cfg.torqueLimit, cfg.torqueLimit);
+ targetJointTorques(i) =
+ std::clamp(targetJointTorques(i), -cfg.torqueLimit, cfg.torqueLimit);
}
//write debug data
{
diff --git a/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.h b/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.h
index 5e108d287d49f1f2115ade20428fdbe8ce016b86..0c0ed889cebc1671c436eee9226dc79457850a25 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.h
@@ -1,7 +1,7 @@
#pragma once
-#include <VirtualRobot/Robot.h>
#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Robot.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
@@ -24,6 +24,7 @@ namespace armarx
Eigen::VectorXf desiredJointPosition;
float torqueLimit;
};
+
//state for debugging
struct Debug
{
@@ -32,6 +33,7 @@ namespace armarx
Eigen::Vector3f tcpDesiredForce;
Eigen::Vector3f tcpDesiredTorque;
};
+
private:
Eigen::VectorXf qpos;
Eigen::VectorXf qvel;
@@ -46,16 +48,16 @@ namespace armarx
VirtualRobot::RobotNodePtr tcp;
const float lambda = 2;
+
public:
Debug dbg;
int bufferSize() const;
void initialize(const VirtualRobot::RobotNodeSetPtr& rns);
- const Eigen::VectorXf& run(
- const Config& cfg,
+ const Eigen::VectorXf&
+ run(const Config& cfg,
std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors,
std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors,
- std::vector<const SensorValue1DoFActuatorPosition*> positionSensors
- );
+ std::vector<const SensorValue1DoFActuatorPosition*> positionSensors);
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.cpp b/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.cpp
index a83b78a522cf70993e5f98cfcffca04981942567..0ed24cb2fb0459f76a6a373d9c1cd3768c733181 100644
--- a/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.cpp
@@ -24,9 +24,8 @@
namespace armarx::WidgetDescription
{
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options)
+ StringComboBoxPtr
+ makeStringSelectionComboBox(std::string name, std::vector<std::string> options)
{
StringComboBoxPtr rns = new StringComboBox;
rns->name = std::move(name);
@@ -35,10 +34,10 @@ namespace armarx::WidgetDescription
return rns;
}
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::set<std::string>& preferredSet)
+ StringComboBoxPtr
+ makeStringSelectionComboBox(std::string name,
+ std::vector<std::string> options,
+ const std::set<std::string>& preferredSet)
{
StringComboBoxPtr rns = makeStringSelectionComboBox(std::move(name), std::move(options));
for (std::size_t i = 0; i < rns->options.size(); ++i)
@@ -52,10 +51,10 @@ namespace armarx::WidgetDescription
return rns;
}
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::string& mostPreferred)
+ StringComboBoxPtr
+ makeStringSelectionComboBox(std::string name,
+ std::vector<std::string> options,
+ const std::string& mostPreferred)
{
StringComboBoxPtr rns = makeStringSelectionComboBox(std::move(name), std::move(options));
for (std::size_t i = 0; i < rns->options.size(); ++i)
@@ -69,13 +68,14 @@ namespace armarx::WidgetDescription
return rns;
}
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::set<std::string>& preferredSet,
- const std::string& mostPreferred)
+ StringComboBoxPtr
+ makeStringSelectionComboBox(std::string name,
+ std::vector<std::string> options,
+ const std::set<std::string>& preferredSet,
+ const std::string& mostPreferred)
{
- StringComboBoxPtr rns = makeStringSelectionComboBox(std::move(name), std::move(options), preferredSet);
+ StringComboBoxPtr rns =
+ makeStringSelectionComboBox(std::move(name), std::move(options), preferredSet);
for (std::size_t i = 0; i < rns->options.size(); ++i)
{
if (mostPreferred == rns->options.at(i))
@@ -86,4 +86,4 @@ namespace armarx::WidgetDescription
}
return rns;
}
-}
+} // namespace armarx::WidgetDescription
diff --git a/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.h b/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.h
index ec9f4c590b56e5b1f984a76c1a7ff2fc92764a8e..e276010f22b2d70b6b6b37fac642991020df968c 100644
--- a/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.h
+++ b/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.h
@@ -22,8 +22,8 @@
#pragma once
-#include <string>
#include <set>
+#include <string>
#include <VirtualRobot/Robot.h>
@@ -32,58 +32,58 @@
namespace armarx::WidgetDescription
{
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options);
+ StringComboBoxPtr makeStringSelectionComboBox(std::string name,
+ std::vector<std::string> options);
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::set<std::string>& preferredSet);
+ StringComboBoxPtr makeStringSelectionComboBox(std::string name,
+ std::vector<std::string> options,
+ const std::set<std::string>& preferredSet);
- inline StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::initializer_list<std::string>& preferredSet)
+ inline StringComboBoxPtr
+ makeStringSelectionComboBox(std::string name,
+ std::vector<std::string> options,
+ const std::initializer_list<std::string>& preferredSet)
{
- return makeStringSelectionComboBox(std::move(name), std::move(options), std::set<std::string> {preferredSet});
+ return makeStringSelectionComboBox(
+ std::move(name), std::move(options), std::set<std::string>{preferredSet});
}
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::string& mostPreferred);
+ StringComboBoxPtr makeStringSelectionComboBox(std::string name,
+ std::vector<std::string> options,
+ const std::string& mostPreferred);
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::set<std::string>& preferredSet,
- const std::string& mostPreferred);
+ StringComboBoxPtr makeStringSelectionComboBox(std::string name,
+ std::vector<std::string> options,
+ const std::set<std::string>& preferredSet,
+ const std::string& mostPreferred);
- inline StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::string& mostPreferred,
- const std::set<std::string>& preferredSet)
+ inline StringComboBoxPtr
+ makeStringSelectionComboBox(std::string name,
+ std::vector<std::string> options,
+ const std::string& mostPreferred,
+ const std::set<std::string>& preferredSet)
{
- return makeStringSelectionComboBox(std::move(name), std::move(options), preferredSet, mostPreferred);
+ return makeStringSelectionComboBox(
+ std::move(name), std::move(options), preferredSet, mostPreferred);
}
- inline StringComboBoxPtr makeRNSComboBox(
- const VirtualRobot::RobotPtr& robot,
- std::string name = "RobotNodeSet",
- const std::set<std::string>& preferredSet = {},
- const std::string& mostPreferred = "")
+ inline StringComboBoxPtr
+ makeRNSComboBox(const VirtualRobot::RobotPtr& robot,
+ std::string name = "RobotNodeSet",
+ const std::set<std::string>& preferredSet = {},
+ const std::string& mostPreferred = "")
{
- return makeStringSelectionComboBox(std::move(name), robot->getRobotNodeSetNames(), preferredSet, mostPreferred);
+ return makeStringSelectionComboBox(
+ std::move(name), robot->getRobotNodeSetNames(), preferredSet, mostPreferred);
}
- inline StringComboBoxPtr makeRobotNodeComboBox(
- const VirtualRobot::RobotPtr& robot,
- std::string name = "RobotNode",
- const std::set<std::string>& preferredSet = {},
- const std::string& mostPreferred = "")
+ inline StringComboBoxPtr
+ makeRobotNodeComboBox(const VirtualRobot::RobotPtr& robot,
+ std::string name = "RobotNode",
+ const std::set<std::string>& preferredSet = {},
+ const std::string& mostPreferred = "")
{
- return makeStringSelectionComboBox(std::move(name), robot->getRobotNodeNames(), preferredSet, mostPreferred);
+ return makeStringSelectionComboBox(
+ std::move(name), robot->getRobotNodeNames(), preferredSet, mostPreferred);
}
-}
+} // namespace armarx::WidgetDescription
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.cpp b/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.cpp
index dbb46ec53cf9dbb6bd086d4639cc077d2eee168d..cdc7bba5ed94a59d8468708a3765a0967a257927 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.cpp
@@ -26,49 +26,50 @@
namespace armarx
{
- const ControlDevicePtr ControlDevice::NullPtr
- {
- nullptr
- };
+ const ControlDevicePtr ControlDevice::NullPtr{nullptr};
- JointController* ControlDevice::getJointEmergencyStopController()
+ JointController*
+ ControlDevice::getJointEmergencyStopController()
{
- ARMARX_CHECK_EXPRESSION(
- jointEmergencyStopController) <<
- "ControlDevice::getJointEmergencyStopController called for a ControlDevice ('"
- << getDeviceName() << "') without a JointController "
- "with the ControlMode ControlModes::EmergencyStop"
- " (add a JointController with this ControlMode)";
+ ARMARX_CHECK_EXPRESSION(jointEmergencyStopController)
+ << "ControlDevice::getJointEmergencyStopController called for a ControlDevice ('"
+ << getDeviceName()
+ << "') without a JointController "
+ "with the ControlMode ControlModes::EmergencyStop"
+ " (add a JointController with this ControlMode)";
return jointEmergencyStopController;
}
- JointController* ControlDevice::getJointStopMovementController()
+ JointController*
+ ControlDevice::getJointStopMovementController()
{
- ARMARX_CHECK_EXPRESSION(
- jointStopMovementController) <<
- "ControlDevice::getJointStopMovementController called for a ControlDevice ('"
- << getDeviceName() << "') without a JointController "
- "with the ControlMode ControlModes::StopMovement"
- " (add a JointController with this ControlMode)";
+ ARMARX_CHECK_EXPRESSION(jointStopMovementController)
+ << "ControlDevice::getJointStopMovementController called for a ControlDevice ('"
+ << getDeviceName()
+ << "') without a JointController "
+ "with the ControlMode ControlModes::StopMovement"
+ " (add a JointController with this ControlMode)";
return jointStopMovementController;
}
- void ControlDevice::rtSetActiveJointController(JointController* jointCtrl)
+ void
+ ControlDevice::rtSetActiveJointController(JointController* jointCtrl)
{
- ARMARX_CHECK_EXPRESSION(jointCtrl) << "Called ControlDevice::rtSetActiveJointController with a nullptr (Don't do this)";
+ ARMARX_CHECK_EXPRESSION(jointCtrl)
+ << "Called ControlDevice::rtSetActiveJointController with a nullptr (Don't do this)";
if (activeJointController == jointCtrl)
{
return;
}
if (jointCtrl->parent != this)
{
- throw std::logic_error
- {
- "ControlDevice(" + getDeviceName() + ")::rtSetActiveJointController: "
+ throw std::logic_error{
+ "ControlDevice(" + getDeviceName() +
+ ")::rtSetActiveJointController: "
"tried to switch to a joint controller from a different ControlDevice "
"(You should only call ControlDevice::rtSetActiveJointController with "
- "JointControllers added to the ControlDevice via ControlDevice::addJointController)"
- };
+ "JointControllers added to the ControlDevice via "
+ "ControlDevice::addJointController)"};
}
if (activeJointController)
{
@@ -78,7 +79,9 @@ namespace armarx
activeJointController = jointCtrl;
}
- void ControlDevice::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ ControlDevice::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
auto activejointCtrl = rtGetActiveJointController();
ARMARX_CHECK_EXPRESSION(activejointCtrl);
@@ -86,38 +89,36 @@ namespace armarx
rtWriteTargetValues(sensorValuesTimestamp, timeSinceLastIteration);
}
- void ControlDevice::addJointController(JointController* jointCtrl)
+ void
+ ControlDevice::addJointController(JointController* jointCtrl)
{
- ARMARX_CHECK_IS_NULL(
- owner) <<
- "The ControlDevice '" << getDeviceName()
- << "' was already added to a RobotUnit! Call addJointController before calling addControlDevice.";
+ ARMARX_CHECK_IS_NULL(owner) << "The ControlDevice '" << getDeviceName()
+ << "' was already added to a RobotUnit! Call "
+ "addJointController before calling addControlDevice.";
ARMARX_DEBUG << "adding Controller " << jointCtrl;
if (!jointCtrl)
{
- throw std::invalid_argument
- {
- "ControlDevice(" + getDeviceName() + ")::addJointController: joint controller is nullptr (Don't try nullptrs as JointController)"
- };
+ throw std::invalid_argument{"ControlDevice(" + getDeviceName() +
+ ")::addJointController: joint controller is nullptr (Don't "
+ "try nullptrs as JointController)"};
}
if (jointCtrl->parent)
{
- throw std::logic_error
- {
- "ControlDevice(" + getDeviceName() + ")::addJointController: tried to add a joint controller multiple times" +
- "(Don't try to add a JointController multiple to a ControlDevice)"
- };
+ throw std::logic_error{
+ "ControlDevice(" + getDeviceName() +
+ ")::addJointController: tried to add a joint controller multiple times" +
+ "(Don't try to add a JointController multiple to a ControlDevice)"};
}
ARMARX_DEBUG << "getControlMode of " << jointCtrl;
const std::string& mode = jointCtrl->getControlMode();
- ARMARX_VERBOSE << "adding joint controller for device '" << getDeviceName() << "' with mode '" << mode << "'" ;
+ ARMARX_VERBOSE << "adding joint controller for device '" << getDeviceName()
+ << "' with mode '" << mode << "'";
if (jointControllers.has(mode))
{
- throw std::invalid_argument
- {
- "ControlDevice(" + getDeviceName() + ")::addJointController: joint controller for mode " + mode + " was already added" +
- "(Don't try to add multiple JointController with the same ControlMode)"
- };
+ throw std::invalid_argument{
+ "ControlDevice(" + getDeviceName() +
+ ")::addJointController: joint controller for mode " + mode + " was already added" +
+ "(Don't try to add multiple JointController with the same ControlMode)"};
}
jointCtrl->parent = this;
ARMARX_DEBUG << "resetting target";
@@ -133,13 +134,12 @@ namespace armarx
}
if (!jointCtrl->getControlTarget())
{
- throw std::logic_error
- {
- "ControlDevice(" + getDeviceName() + ")::addJointController: The JointController has no ControlTarget. (mode = " + mode + ")" +
- "(Don't try to add JointController without a ControlTarget)"
- };
+ throw std::logic_error{
+ "ControlDevice(" + getDeviceName() +
+ ")::addJointController: The JointController has no ControlTarget. (mode = " + mode +
+ ")" + "(Don't try to add JointController without a ControlTarget)"};
}
- const std::hash<std::string> hash {};
+ const std::hash<std::string> hash{};
jointCtrl->controlModeHash = hash(jointCtrl->getControlMode());
jointCtrl->hardwareControlModeHash = hash(jointCtrl->getHardwareControlMode());
jointControllers.add(mode, std::move(jointCtrl));
@@ -147,12 +147,14 @@ namespace armarx
ARMARX_CHECK_IS_NULL(owner);
}
- RobotUnitModule::Devices* ControlDevice::getOwner() const
+ RobotUnitModule::Devices*
+ ControlDevice::getOwner() const
{
return owner;
}
- std::map<std::string, std::string> ControlDevice::getJointControllerToTargetTypeNameMap() const
+ std::map<std::string, std::string>
+ ControlDevice::getJointControllerToTargetTypeNameMap() const
{
std::map<std::string, std::string> map;
for (const auto& name : jointControllers.keys())
@@ -162,4 +164,4 @@ namespace armarx
return map;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h b/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h
index 9ad894135f395848ec4c5e6aaba6288e98a6f930..692d032deeccd2624d4d97ab5b22dce4a451b172 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h
@@ -22,14 +22,14 @@
#pragma once
-#include <string>
-#include <memory>
#include <atomic>
+#include <memory>
+#include <string>
-#include"DeviceBase.h"
-#include "../util.h"
#include "../ControlModes.h"
#include "../JointControllers/JointController.h"
+#include "../util.h"
+#include "DeviceBase.h"
namespace armarx::RobotUnitModule
{
@@ -40,13 +40,16 @@ namespace armarx::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";
-}
+ const static std::string CreateNoDefaultController =
+ "ControlDeviceTag_CreateNoDefaultController";
+ const static std::string ForcePositionThroughVelocity =
+ "ControlDeviceTag_ForcePositionThroughVelocity";
+} // namespace armarx::ControlDeviceTags
namespace armarx
{
TYPEDEF_PTRS_SHARED(ControlDevice);
+
/**
* @brief The ControlDevice class represents a logical unit accepting commands via its JointControllers.
*
@@ -57,14 +60,17 @@ namespace armarx
* 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
+ 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} {}
+ ControlDevice(const std::string& name) : DeviceBase{name}
+ {
+ }
+
//controller management
/// @return the jointEmergencyStopController of this ControlDevice
JointController* rtGetJointEmergencyStopController();
@@ -97,17 +103,24 @@ namespace armarx
* @param timeSinceLastIteration
* @see writeTargetValues
*/
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration);
/**
* @brief This is a hook for implementations to write the setpoints to a bus.
* @param sensorValuesTimestamp The timestamp of the current \ref SensorValueBase "SensorValues"
* @param timeSinceLastIteration The time delta between the last two updates of \ref SensorValueBase "SensorValues"
*/
- virtual void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) {}
+ virtual void
+ rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
+ {
+ }
+
RobotUnitModule::Devices* getOwner() const;
std::map<std::string, std::string> getJointControllerToTargetTypeNameMap() const;
+
protected:
/**
* @brief adds the Joint controller to this ControlDevice
@@ -116,81 +129,93 @@ namespace armarx
*/
void addJointController(JointController* jointCtrl);
- ControlTargetBase::ControlDeviceAccessToken getControlTargetAccessToken() const
+ ControlTargetBase::ControlDeviceAccessToken
+ getControlTargetAccessToken() const
{
return {};
}
+
private:
friend class RobotUnitModule::Devices;
/// @brief The owning \ref RobotUnit. (is set when this \ref SensorDevice is added to the \ref RobotUnit)
- std::atomic<RobotUnitModule::Devices*> owner {nullptr};
+ std::atomic<RobotUnitModule::Devices*> owner{nullptr};
/// @brief The \ref JointController "JointControllers" managed by this \ref ControlDevice
KeyValueVector<std::string, JointController*> jointControllers;
/// @brief The currently executed \ref JointController
- JointController* activeJointController {nullptr};
+ JointController* activeJointController{nullptr};
/// @brief This \ref ControlDevice "ControlDevice's" emergency stop controller
- JointController* jointEmergencyStopController {nullptr};
+ JointController* jointEmergencyStopController{nullptr};
/// @brief This \ref ControlDevice "ControlDevice's" stop movement controller
- JointController* jointStopMovementController {nullptr};
+ JointController* jointStopMovementController{nullptr};
};
-}
+} // namespace armarx
//inline functions
namespace armarx
{
- inline JointController* ControlDevice::rtGetJointEmergencyStopController()
+ inline JointController*
+ ControlDevice::rtGetJointEmergencyStopController()
{
return jointEmergencyStopController;
}
- inline JointController* ControlDevice::rtGetJointStopMovementController()
+ inline JointController*
+ ControlDevice::rtGetJointStopMovementController()
{
return jointStopMovementController;
}
- inline JointController* ControlDevice::rtGetActiveJointController()
+ inline JointController*
+ ControlDevice::rtGetActiveJointController()
{
return activeJointController;
}
- inline const std::vector<JointController*>& ControlDevice::rtGetJointControllers()
+ inline const std::vector<JointController*>&
+ ControlDevice::rtGetJointControllers()
{
return jointControllers.values();
}
- inline std::vector<const JointController*> ControlDevice::getJointControllers() const
+ inline std::vector<const JointController*>
+ ControlDevice::getJointControllers() const
{
return {jointControllers.values().begin(), jointControllers.values().end()};
}
- inline JointController* ControlDevice::getJointController(const std::string& mode)
+ inline JointController*
+ ControlDevice::getJointController(const std::string& mode)
{
return jointControllers.at(mode, nullptr);
}
- inline const JointController* ControlDevice::getJointController(const std::string& mode) const
+ inline const JointController*
+ ControlDevice::getJointController(const std::string& mode) const
{
return jointControllers.at(mode, nullptr);
}
- inline JointController* ControlDevice::getJointController(std::size_t i)
+ inline JointController*
+ ControlDevice::getJointController(std::size_t i)
{
return jointControllers.at(i);
}
- inline const JointController* ControlDevice::getJointController(std::size_t i) const
+ inline const JointController*
+ ControlDevice::getJointController(std::size_t i) const
{
return jointControllers.at(i);
}
- inline bool ControlDevice::hasJointController(const std::string& mode) const
+ inline bool
+ ControlDevice::hasJointController(const std::string& mode) const
{
return jointControllers.has(mode);
}
- inline const std::vector<std::string>& ControlDevice::getControlModes() const
+ inline const std::vector<std::string>&
+ ControlDevice::getControlModes() const
{
return jointControllers.keys();
}
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h b/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h
index 35958d52d5f984b14e5644e2372990d103eb935b..252be38871a65753da6966130a246436368a71ff 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h
@@ -33,6 +33,7 @@ namespace armarx
DeviceBase(const std::string& name) : deviceName{name}
{
}
+
virtual ~DeviceBase() = default;
/// @return The Device's name
const std::string& getDeviceName() const;
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp b/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp
index 57f7cf102806a02d1bf502eaf5920e6b43dfff3f..0a11b09754f6a47f7b52e3fe0958f66aeeb034bd 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp
@@ -1,8 +1,9 @@
#include "GlobalRobotPoseSensorDevice.h"
-#include "ArmarXCore/core/logging/Logging.h"
#include <SimoxUtility/math/convert/pos_rpy_to_mat4f.h>
+#include "ArmarXCore/core/logging/Logging.h"
+
namespace armarx
{
SensorValueBase::SensorValueInfo<SensorValueGlobalPoseCorrection>
@@ -14,7 +15,6 @@ namespace armarx
return svi;
}
-
bool
SensorValueGlobalPoseCorrection::isAvailable() const
{
@@ -30,42 +30,36 @@ namespace armarx
return svi;
}
-
bool
SensorValueGlobalRobotPose::isAvailable() const
{
return not global_T_root.isIdentity();
}
-
std::string
GlobalRobotPoseCorrectionSensorDevice::DeviceName()
{
return "GlobalRobotPoseCorrectionSensorDevice";
}
-
GlobalRobotPoseCorrectionSensorDevice::GlobalRobotPoseCorrectionSensorDevice() :
DeviceBase(DeviceName()), SensorDevice(DeviceName())
{
transformationBuffer.reinitAllBuffers(SensorValueType::Transformation::Identity());
}
-
const SensorValueBase*
GlobalRobotPoseCorrectionSensorDevice::getSensorValue() const
{
return &sensor;
}
-
std::string
GlobalRobotPoseCorrectionSensorDevice::getReportingFrame() const
{
return GlobalFrame;
}
-
void
GlobalRobotPoseCorrectionSensorDevice::rtReadSensorValues(
const IceUtil::Time& sensorValuesTimestamp,
@@ -74,7 +68,6 @@ namespace armarx
sensor.global_T_odom = transformationBuffer.getUpToDateReadBuffer();
}
-
void
GlobalRobotPoseCorrectionSensorDevice::updateGlobalPositionCorrection(
const SensorValueType::Transformation& global_T_odom)
@@ -83,54 +76,46 @@ namespace armarx
transformationBuffer.commitWrite();
}
-
std::string
GlobalRobotPoseSensorDevice::DeviceName()
{
return "GlobalRobotPoseSensorDevice";
}
-
GlobalRobotPoseSensorDevice::GlobalRobotPoseSensorDevice() :
DeviceBase(DeviceName()), SensorDevice(DeviceName())
{
}
-
const SensorValueBase*
GlobalRobotPoseSensorDevice::getSensorValue() const
{
return &sensor;
}
-
std::string
GlobalRobotPoseSensorDevice::getReportingFrame() const
{
return GlobalFrame;
}
-
void
GlobalRobotPoseSensorDevice::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
{
}
-
std::string
GlobalRobotLocalizationSensorDevice::DeviceName()
{
return "GlobalRobotLocalizationSensorDevice";
}
-
GlobalRobotLocalizationSensorDevice::GlobalRobotLocalizationSensorDevice() :
DeviceBase(DeviceName()), SensorDevice(DeviceName())
{
}
-
void
GlobalRobotLocalizationSensorDevice::rtReadSensorValues(
const IceUtil::Time& sensorValuesTimestamp,
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h b/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h
index c3cd19f4a3777a6903ceca8b4a1e8660f25b617b..aa6a2bce15c463861983fe97f12becd4bf4dd918 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h
@@ -38,7 +38,6 @@ namespace armarx
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
};
-
class SensorValueGlobalRobotPose : virtual public SensorValueBase
{
public:
@@ -53,6 +52,7 @@ namespace armarx
};
TYPEDEF_PTRS_SHARED(GlobalRobotPoseCorrectionSensorDevice);
+
class GlobalRobotPoseCorrectionSensorDevice : virtual public SensorDevice
{
public:
@@ -77,8 +77,8 @@ namespace armarx
TripleBuffer<SensorValueType::Transformation> transformationBuffer;
};
-
TYPEDEF_PTRS_SHARED(GlobalRobotPoseSensorDevice);
+
class GlobalRobotPoseSensorDevice : virtual public SensorDevice
{
public:
@@ -99,8 +99,8 @@ namespace armarx
SensorValueType sensor;
};
-
TYPEDEF_PTRS_SHARED(GlobalRobotPoseSensorDevice);
+
class GlobalRobotLocalizationSensorDevice : virtual public GlobalRobotPoseSensorDevice
{
public:
@@ -113,7 +113,6 @@ namespace armarx
const SensorValueGlobalPoseCorrection* sensorGlobalPositionCorrection;
const SensorValueHolonomicPlatformRelativePosition* sensorRelativePosition;
-
};
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/RTThreadTimingsSensorDevice.h b/source/RobotAPI/components/units/RobotUnit/Devices/RTThreadTimingsSensorDevice.h
index 89c0fab1ba6d9f042a7351233a14a86d7641edc0..d1df912e12771ae8c09befd2c6dd4832897fadad 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/RTThreadTimingsSensorDevice.h
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/RTThreadTimingsSensorDevice.h
@@ -22,8 +22,8 @@
#pragma once
-#include"SensorDevice.h"
#include "../SensorValues/SensorValueRTThreadTimings.h"
+#include "SensorDevice.h"
namespace armarx::RobotUnitModule
{
@@ -34,10 +34,13 @@ namespace armarx
{
TYPEDEF_PTRS_SHARED(RTThreadTimingsSensorDevice);
- class RTThreadTimingsSensorDevice: virtual public SensorDevice
+ class RTThreadTimingsSensorDevice : virtual public SensorDevice
{
public:
- RTThreadTimingsSensorDevice(const std::string& name): DeviceBase(name), SensorDevice(name) {}
+ RTThreadTimingsSensorDevice(const std::string& name) : DeviceBase(name), SensorDevice(name)
+ {
+ }
+
const SensorValueBase* getSensorValue() const override = 0;
virtual void rtMarkRtLoopStart() = 0;
@@ -45,6 +48,7 @@ namespace armarx
virtual void rtMarkRtBusSendReceiveStart() = 0;
virtual void rtMarkRtBusSendReceiveEnd() = 0;
+
protected:
friend class RobotUnit;
friend class RobotUnitModule::ControlThread;
@@ -70,68 +74,78 @@ namespace armarx
virtual void rtMarkRtResetAllTargetsStart() = 0;
virtual void rtMarkRtResetAllTargetsEnd() = 0;
-
};
- template<class SensorValueType = SensorValueRTThreadTimings>
- class RTThreadTimingsSensorDeviceImpl: public RTThreadTimingsSensorDevice
+ 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) {}
- const SensorValueBase* getSensorValue() const override
+ 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)
+ {
+ }
+
+ const SensorValueBase*
+ getSensorValue() const override
{
return &value;
}
SensorValueType value;
- void rtMarkRtLoopStart() override
+ void
+ rtMarkRtLoopStart() override
{
const IceUtil::Time now = TimeUtil::GetTime(true);
value.rtLoopRoundTripTime = now - rtLoopStart;
rtLoopStart = now;
}
- void rtMarkRtLoopPreSleep() override
+
+ void
+ rtMarkRtLoopPreSleep() override
{
value.rtLoopDuration = TimeUtil::GetTime(true) - rtLoopStart;
}
- void rtMarkRtBusSendReceiveStart() override
+
+ void
+ rtMarkRtBusSendReceiveStart() override
{
const IceUtil::Time now = TimeUtil::GetTime(true);
value.rtBusSendReceiveRoundTripTime = now - rtBusSendReceiveStart;
rtBusSendReceiveStart = now;
}
- void rtMarkRtBusSendReceiveEnd() override
+
+ void
+ rtMarkRtBusSendReceiveEnd() override
{
value.rtBusSendReceiveDuration = TimeUtil::GetTime(true) - rtBusSendReceiveStart;
}
+
protected:
-#define make_markRT_X_Start_End(name) \
- virtual void rtMarkRt##name##Start() override \
- { \
- const IceUtil::Time now = TimeUtil::GetTime(true); \
- value.rt##name##RoundTripTime = now - rt##name##Start; \
- rt##name##Start=now; \
- } \
- virtual void rtMarkRt##name##End() override \
- { \
- value.rt##name##Duration=TimeUtil::GetTime(true) - rt##name##Start; \
+#define make_markRT_X_Start_End(name) \
+ virtual void rtMarkRt##name##Start() override \
+ { \
+ const IceUtil::Time now = TimeUtil::GetTime(true); \
+ value.rt##name##RoundTripTime = now - rt##name##Start; \
+ rt##name##Start = now; \
+ } \
+ virtual void rtMarkRt##name##End() override \
+ { \
+ value.rt##name##Duration = TimeUtil::GetTime(true) - rt##name##Start; \
}
- make_markRT_X_Start_End(SwitchControllerSetup)
- make_markRT_X_Start_End(RunNJointControllers)
- make_markRT_X_Start_End(HandleInvalidTargets)
- make_markRT_X_Start_End(RunJointControllers)
- make_markRT_X_Start_End(ReadSensorDeviceValues)
- make_markRT_X_Start_End(UpdateSensorAndControlBuffer)
- make_markRT_X_Start_End(ResetAllTargets)
+ make_markRT_X_Start_End(SwitchControllerSetup) make_markRT_X_Start_End(RunNJointControllers)
+ make_markRT_X_Start_End(HandleInvalidTargets)
+ make_markRT_X_Start_End(RunJointControllers)
+ 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:
- IceUtil::Time rtSwitchControllerSetupStart;
+ protected : IceUtil::Time rtSwitchControllerSetupStart;
IceUtil::Time rtRunNJointControllersStart;
IceUtil::Time rtHandleInvalidTargetsStart;
IceUtil::Time rtRunJointControllersStart;
@@ -142,5 +156,4 @@ namespace armarx
IceUtil::Time rtLoopStart;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.cpp b/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.cpp
index 956e334a81e46441e3850779a0d40e3650c7fe6c..db2c8c52d33f2f036baa8c70001615e18b10d0fe 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.cpp
@@ -24,9 +24,6 @@
namespace armarx
{
- const SensorDevicePtr SensorDevice::NullPtr
- {
- nullptr
- };
+ const SensorDevicePtr SensorDevice::NullPtr{nullptr};
}
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h b/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h
index a35ae8b8d359fffc4c963b1a0a426b68902eea51..c0a7ed43b74b8375a208c2afbe53dc205b26b7a8 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h
@@ -22,10 +22,11 @@
#pragma once
-#include"DeviceBase.h"
+#include <IceUtil/Time.h>
+
#include "../SensorValues/SensorValueBase.h"
#include "../util.h"
-#include <IceUtil/Time.h>
+#include "DeviceBase.h"
namespace armarx::RobotUnitModule
{
@@ -40,6 +41,7 @@ namespace armarx::SensorDeviceTags
namespace armarx
{
TYPEDEF_PTRS_SHARED(SensorDevice);
+
/**
* @brief This class represents some part of the robot providing sensor values.
*
@@ -54,20 +56,24 @@ namespace armarx
* \li Level of a battery power supply
* \li 3d Velocity of a mobile Platform
*/
- class SensorDevice: public virtual DeviceBase
+ 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} {}
+ SensorDevice(const std::string& name) : DeviceBase{name}
+ {
+ }
/// @return The SensorDevice's sensor value
virtual const SensorValueBase* getSensorValue() const = 0;
/// @return The SensorDevice's sensor value casted to the given type (may be nullptr)
- template<class T>
- const T* getSensorValue() const
+ template <class T>
+ const T*
+ getSensorValue() const
{
return getSensorValue()->asA<const T*>();
}
@@ -89,44 +95,54 @@ namespace armarx
* @param sensorValuesTimestamp The current timestamp
* @param timeSinceLastIteration The time delta since the last call
*/
- virtual void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) {}
+ virtual void
+ rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
+ {
+ }
private:
friend class RobotUnitModule::Devices;
/// @brief The owning \ref RobotUnit. (is set when this \ref SensorDevice is added to the \ref RobotUnit)
- std::atomic<RobotUnitModule::Devices*> owner {nullptr};
+ std::atomic<RobotUnitModule::Devices*> owner{nullptr};
};
- template<class SensorValueType>
+ template <class SensorValueType>
class SensorDeviceTemplate : public virtual SensorDevice
{
- static_assert(std::is_base_of<SensorValueBase, SensorValueType>::value, "SensorValueType has to inherit SensorValueBase");
+ static_assert(std::is_base_of<SensorValueBase, SensorValueType>::value,
+ "SensorValueType has to inherit SensorValueBase");
+
public:
- SensorDeviceTemplate(const std::string& name): DeviceBase(name), SensorDevice(name) {}
+ SensorDeviceTemplate(const std::string& name) : DeviceBase(name), SensorDevice(name)
+ {
+ }
const SensorValueType* getSensorValue() const final override;
SensorValueType sensorValue;
};
-}
+} // namespace armarx
//inline functions
namespace armarx
{
- inline std::string SensorDevice::getSensorValueType(bool withoutNamespaceSpecifier) const
+ inline std::string
+ SensorDevice::getSensorValueType(bool withoutNamespaceSpecifier) const
{
return getSensorValue()->getSensorValueType(withoutNamespaceSpecifier);
}
- inline std::string SensorDevice::getReportingFrame() const
+ inline std::string
+ SensorDevice::getReportingFrame() const
{
return {};
}
- template<class SensorValueType>
- inline const SensorValueType* SensorDeviceTemplate<SensorValueType>::getSensorValue() const
+ template <class SensorValueType>
+ inline const SensorValueType*
+ SensorDeviceTemplate<SensorValueType>::getSensorValue() const
{
return &sensorValue;
}
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.cpp b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.cpp
index 84e9891176c90ba59443714c444276357f4f30f9..425a51ee3b525253da475dccb6d66bb4728f06c0 100644
--- a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.cpp
@@ -26,14 +26,17 @@
namespace armarx
{
- ControlDevice& JointController::getParent() const
+ ControlDevice&
+ JointController::getParent() const
{
ARMARX_CHECK_EXPRESSION(parent) << "This JointController is not owned by a ControlDevice";
return *parent;
}
- StringVariantBaseMap JointController::publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const
+ StringVariantBaseMap
+ JointController::publish(const DebugDrawerInterfacePrx& draw,
+ const DebugObserverInterfacePrx& observer) const
{
- return StringVariantBaseMap {};
+ return StringVariantBaseMap{};
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h
index 869ad5163e781a218795dcedf119f1b096be9ccb..b72d8a939c137c94e14ff8fa8da7163e13d95365 100644
--- a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h
@@ -22,24 +22,26 @@
#pragma once
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h>
+#include <atomic>
+#include <memory>
#include <Ice/ProxyHandle.h>
-#include <memory>
-#include <atomic>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h>
namespace IceProxy::armarx
{
class DebugDrawerInterface;
class DebugObserverInterface;
-}
+} // namespace IceProxy::armarx
namespace armarx
{
class ControlDevice;
- typedef ::IceInternal::ProxyHandle< ::IceProxy::armarx::DebugDrawerInterface> DebugDrawerInterfacePrx;
- typedef ::IceInternal::ProxyHandle< ::IceProxy::armarx::DebugObserverInterface> DebugObserverInterfacePrx;
+ typedef ::IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface>
+ DebugDrawerInterfacePrx;
+ typedef ::IceInternal::ProxyHandle<::IceProxy::armarx::DebugObserverInterface>
+ DebugObserverInterfacePrx;
/**
* @brief The JointController class represents one joint in one control mode.
@@ -56,8 +58,9 @@ namespace armarx
virtual const ControlTargetBase* getControlTarget() const;
/// @return The \ref JointController's \ref ControlTargetBase "ControlTarget" casted to the given type (may be nullptr)
- template<class T>
- const T* getControlTarget() const
+ template <class T>
+ const T*
+ getControlTarget() const
{
return getControlTarget()->asA<const T*>();
}
@@ -88,36 +91,43 @@ namespace armarx
* with "{ControlDeviceName}_{ControlModeName}_" and are used as keys of the datafields.
*
*/
- virtual StringVariantBaseMap publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const;
+ virtual StringVariantBaseMap publish(const DebugDrawerInterfacePrx& draw,
+ const DebugObserverInterfacePrx& observer) const;
ControlDevice& rtGetParent() const;
- template<class T>
- T& rtGetParent() const
+
+ template <class T>
+ T&
+ rtGetParent() const
{
return dynamic_cast<T&>(rtGetParent);
}
+
protected:
//called by the owning ControlDevice
/// @brief called when this JointController is run
- virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
+ virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) = 0;
/// @brief called when this JointController is activated
virtual void rtPreActivateController();
virtual void rtPostDeactivateController();
/// @brief called when this JointController is deactivated
private:
friend class ControlDevice;
- std::atomic_bool activated {false};
- ControlDevice* parent {nullptr};
- std::size_t controlModeHash {0};
- std::size_t hardwareControlModeHash {0};
+ std::atomic_bool activated{false};
+ ControlDevice* parent{nullptr};
+ std::size_t controlModeHash{0};
+ std::size_t hardwareControlModeHash{0};
void rtActivate();
void rtDeactivate();
};
- template<class ControlTargetType>
+ template <class ControlTargetType>
class JointControllerTemplate : public virtual JointController
{
- static_assert(std::is_base_of<ControlTargetBase, ControlTargetType>::value, "ControlTargetType has to inherit SensorValueBase");
+ static_assert(std::is_base_of<ControlTargetBase, ControlTargetType>::value,
+ "ControlTargetType has to inherit SensorValueBase");
+
public:
using JointController::JointController;
@@ -125,87 +135,101 @@ namespace armarx
ControlTargetType controlTarget;
};
-}
+} // namespace armarx
//inline functions
namespace armarx
{
- inline const ControlTargetBase* JointController::getControlTarget() const
+ inline const ControlTargetBase*
+ JointController::getControlTarget() const
{
- return const_cast<const ControlTargetBase*>(const_cast<JointController*>(this)->getControlTarget());
+ return const_cast<const ControlTargetBase*>(
+ const_cast<JointController*>(this)->getControlTarget());
}
- inline void JointController::rtResetTarget()
+ inline void
+ JointController::rtResetTarget()
{
getControlTarget()->reset();
}
- inline bool JointController::rtIsTargetValid() const
+ inline bool
+ JointController::rtIsTargetValid() const
{
return getControlTarget()->isValid();
}
- inline const std::string& JointController::getControlMode() const
+ inline const std::string&
+ JointController::getControlMode() const
{
return getControlTarget()->getControlMode();
}
- inline std::string JointController::getHardwareControlMode() const
+ inline std::string
+ JointController::getHardwareControlMode() const
{
return "";
}
- inline std::size_t JointController::getControlModeHash() const
+ inline std::size_t
+ JointController::getControlModeHash() const
{
return controlModeHash;
}
- inline std::size_t JointController::rtGetControlModeHash() const
+ inline std::size_t
+ JointController::rtGetControlModeHash() const
{
return controlModeHash;
}
- inline std::size_t JointController::getHardwareControlModeHash() const
+ inline std::size_t
+ JointController::getHardwareControlModeHash() const
{
return hardwareControlModeHash;
}
- inline std::size_t JointController::rtGetHardwareControlModeHash() const
+ inline std::size_t
+ JointController::rtGetHardwareControlModeHash() const
{
return hardwareControlModeHash;
}
- inline ControlDevice& JointController::rtGetParent() const
+ inline ControlDevice&
+ JointController::rtGetParent() const
{
return *parent;
}
- inline void JointController::rtPreActivateController()
+ inline void
+ JointController::rtPreActivateController()
{
-
}
- inline void JointController::rtPostDeactivateController()
+ inline void
+ JointController::rtPostDeactivateController()
{
-
}
- inline void JointController::rtActivate()
+ inline void
+ JointController::rtActivate()
{
rtPreActivateController();
activated = true;
}
- inline void JointController::rtDeactivate()
+ inline void
+ JointController::rtDeactivate()
{
activated = false;
rtPostDeactivateController();
rtResetTarget();
}
- template<class ControlTargetType>
- inline ControlTargetType* JointControllerTemplate<ControlTargetType>::getControlTarget()
+ template <class ControlTargetType>
+ inline ControlTargetType*
+ JointControllerTemplate<ControlTargetType>::getControlTarget()
{
return &controlTarget;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
index ff1afaf689c23ebd13e118965ff0848ebf1300c7..15baf198eb03202a929341675f01ef1ab831d234 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
@@ -1,22 +1,22 @@
+#include "NJointCartesianNaturalPositionController.h"
+
+#include <iomanip>
+
+#include <VirtualRobot/math/Helpers.h>
+
#include <ArmarXCore/util/CPPUtility/trace.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
-
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
-#include <VirtualRobot/math/Helpers.h>
-
-#include <iomanip>
-
-#include "NJointCartesianNaturalPositionController.h"
-
namespace armarx
{
- std::string vec2str(const std::vector<float>& vec)
+ std::string
+ vec2str(const std::vector<float>& vec)
{
std::stringstream ss;
for (size_t i = 0; i < vec.size(); i++)
@@ -29,7 +29,9 @@ namespace armarx
}
return ss.str();
}
- std::ostream& operator<<(std::ostream& out, const CartesianNaturalPositionControllerConfig& cfg)
+
+ std::ostream&
+ operator<<(std::ostream& out, const CartesianNaturalPositionControllerConfig& cfg)
{
out << "maxPositionAcceleration " << cfg.maxPositionAcceleration << '\n'
<< "maxOrientationAcceleration " << cfg.maxOrientationAcceleration << '\n'
@@ -53,10 +55,12 @@ namespace armarx
return out;
}
+ NJointControllerRegistration<NJointCartesianNaturalPositionController>
+ registrationControllerNJointCartesianNaturalPositionController(
+ "NJointCartesianNaturalPositionController");
- NJointControllerRegistration<NJointCartesianNaturalPositionController> registrationControllerNJointCartesianNaturalPositionController("NJointCartesianNaturalPositionController");
-
- std::string NJointCartesianNaturalPositionController::getClassName(const Ice::Current&) const
+ std::string
+ NJointCartesianNaturalPositionController::getClassName(const Ice::Current&) const
{
return "NJointCartesianNaturalPositionController";
}
@@ -77,14 +81,11 @@ namespace armarx
ARMARX_CHECK_NOT_NULL(_rtRobot);
_rtRns = _rtRobot->getRobotNodeSet(config->rns);
- ARMARX_CHECK_NOT_NULL(_rtRns)
- << "No rns " << config->rns;
+ ARMARX_CHECK_NOT_NULL(_rtRns) << "No rns " << config->rns;
_rtTcp = _rtRns->getTCP();
- ARMARX_CHECK_NOT_NULL(_rtTcp)
- << "No tcp in rns " << config->rns;
+ ARMARX_CHECK_NOT_NULL(_rtTcp) << "No tcp in rns " << config->rns;
_rtElbow = _rtRobot->getRobotNode(config->elbowNode);
- ARMARX_CHECK_NOT_NULL(_rtElbow)
- << "No elbow node " << config->elbowNode;
+ ARMARX_CHECK_NOT_NULL(_rtElbow) << "No elbow node " << config->elbowNode;
_rtRobotRoot = _rtRobot->getRootNode();
}
@@ -95,8 +96,7 @@ namespace armarx
if (!config->ftName.empty())
{
const auto svalFT = useSensorValue<SensorValueForceTorque>(config->ftName);
- ARMARX_CHECK_NOT_NULL(svalFT)
- << "No sensor value of name " << config->ftName;
+ ARMARX_CHECK_NOT_NULL(svalFT) << "No sensor value of name " << config->ftName;
_rtForceSensor = &(svalFT->force);
_rtTorqueSensor = &(svalFT->force);
@@ -105,26 +105,25 @@ namespace armarx
const auto reportFrameName = sdev->getReportingFrame();
ARMARX_CHECK_EXPRESSION(!reportFrameName.empty())
- << VAROUT(sdev->getReportingFrame());
+ << VAROUT(sdev->getReportingFrame());
_rtFT = _rtRobot->getRobotNode(reportFrameName);
ARMARX_CHECK_NOT_NULL(_rtFT)
- << "No sensor report frame '" << reportFrameName
- << "' in robot";
+ << "No sensor report frame '" << reportFrameName << "' in robot";
}
}
//ARMARX_IMPORTANT << "got sensor";
//ctrl
{
- _rtJointVelocityFeedbackBuffer = Eigen::VectorXf::Zero(static_cast<int>(_rtRns->getSize()));
+ _rtJointVelocityFeedbackBuffer =
+ Eigen::VectorXf::Zero(static_cast<int>(_rtRns->getSize()));
_rtPosController = std::make_unique<CartesianNaturalPositionController>(
- _rtRns,
- _rtElbow,
- config->runCfg.maxPositionAcceleration,
- config->runCfg.maxOrientationAcceleration,
- config->runCfg.maxNullspaceAcceleration
- );
+ _rtRns,
+ _rtElbow,
+ config->runCfg.maxPositionAcceleration,
+ config->runCfg.maxOrientationAcceleration,
+ config->runCfg.maxNullspaceAcceleration);
_rtPosController->setConfig(config->runCfg);
ARMARX_IMPORTANT << "controller config: " << config->runCfg;
@@ -132,7 +131,8 @@ namespace armarx
for (size_t i = 0; i < _rtRns->getSize(); ++i)
{
std::string jointName = _rtRns->getNode(i)->getName();
- auto ct = useControlTarget<ControlTarget1DoFActuatorVelocity>(jointName, ControlModes::Velocity1DoF);
+ auto ct = useControlTarget<ControlTarget1DoFActuatorVelocity>(
+ jointName, ControlModes::Velocity1DoF);
auto sv = useSensorValue<SensorValue1DoFActuatorVelocity>(jointName);
ARMARX_CHECK_NOT_NULL(ct);
ARMARX_CHECK_NOT_NULL(sv);
@@ -155,7 +155,8 @@ namespace armarx
//ARMARX_IMPORTANT << "got visu";
}
- void NJointCartesianNaturalPositionController::rtPreActivateController()
+ void
+ NJointCartesianNaturalPositionController::rtPreActivateController()
{
//_publishIsAtTarget = false;
//_rtForceOffset = Eigen::Vector3f::Zero();
@@ -169,11 +170,13 @@ namespace armarx
_publish.elbPosVel = 0;
//_publish.targetReached = false;
//_publishForceThreshold = _rtForceThreshold;
- _rtPosController->setTarget(_rtTcp->getPoseInRootFrame(), _rtElbow->getPositionInRootFrame());
-
+ _rtPosController->setTarget(_rtTcp->getPoseInRootFrame(),
+ _rtElbow->getPositionInRootFrame());
}
- void NJointCartesianNaturalPositionController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ NJointCartesianNaturalPositionController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
auto& rt2nonrtBuf = _tripRt2NonRt.getWriteBuffer();
@@ -218,7 +221,6 @@ namespace armarx
_rtPosController->setNullspaceTarget(r.nullspaceTarget);
}
r.clearRequested = false;
-
}
}
if (_tripBufFFvel.updateReadBuffer())
@@ -286,7 +288,8 @@ namespace armarx
rt2nonrtBuf.currentFT = ft;
rt2nonrtBuf.averageFT = ftAvg;
- if ((_rtFTlimit.force > 0 && ft.force.norm() > _rtFTlimit.force) || (_rtFTlimit.torque > 0 && ft.torque.norm() > _rtFTlimit.torque))
+ if ((_rtFTlimit.force > 0 && ft.force.norm() > _rtFTlimit.force) ||
+ (_rtFTlimit.torque > 0 && ft.torque.norm() > _rtFTlimit.torque))
{
_rtStopConditionReached = true;
_publishIsAtForceLimit = true;
@@ -319,7 +322,8 @@ namespace armarx
rt2nonrtBuf.tcp = _rtTcp->getPoseInRootFrame();
rt2nonrtBuf.elb = _rtElbow->getPoseInRootFrame();
rt2nonrtBuf.tcpTarg = _rtPosController->getCurrentTarget();
- rt2nonrtBuf.elbTarg = ::math::Helpers::CreatePose(_rtPosController->getCurrentElbowTarget(), Eigen::Matrix3f::Identity());
+ rt2nonrtBuf.elbTarg = ::math::Helpers::CreatePose(_rtPosController->getCurrentElbowTarget(),
+ Eigen::Matrix3f::Identity());
if (_rtStopConditionReached)
@@ -328,12 +332,14 @@ namespace armarx
}
else
{
- VirtualRobot::IKSolver::CartesianSelection mode = _rtSetOrientation ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position;
+ VirtualRobot::IKSolver::CartesianSelection mode =
+ _rtSetOrientation ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position;
_rtPosController->setNullSpaceControl(_nullspaceControlEnabled);
if (_rtFFvel.use)
{
- if (_rtFFvelLastUpdateMS + _rtFFvelMaxAgeMS > sensorValuesTimestamp.toMilliSeconds())
+ if (_rtFFvelLastUpdateMS + _rtFFvelMaxAgeMS >
+ sensorValuesTimestamp.toMilliSeconds())
{
_rtPosController->setFeedForwardVelocity(_rtFFvel.feedForwardVelocity);
}
@@ -344,7 +350,8 @@ namespace armarx
}
}
- const Eigen::VectorXf& goal = _rtPosController->calculate(timeSinceLastIteration.toSecondsDouble(), mode);
+ const Eigen::VectorXf& goal =
+ _rtPosController->calculate(timeSinceLastIteration.toSecondsDouble(), mode);
const Eigen::VectorXf actualTcpVel = _rtPosController->actualTcpVel(goal);
const Eigen::VectorXf actualElbVel = _rtPosController->actualElbVel(goal);
@@ -364,12 +371,15 @@ namespace armarx
_tripRt2NonRt.commitWrite();
}
- void NJointCartesianNaturalPositionController::rtPostDeactivateController()
+ void
+ NJointCartesianNaturalPositionController::rtPostDeactivateController()
{
-
}
- void NJointCartesianNaturalPositionController::setConfig(const CartesianNaturalPositionControllerConfig& cfg, const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::setConfig(
+ const CartesianNaturalPositionControllerConfig& cfg,
+ const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufCfg.getWriteBuffer();
@@ -379,7 +389,11 @@ namespace armarx
ARMARX_IMPORTANT << "set new config\n" << cfg;
}
- void NJointCartesianNaturalPositionController::setTarget(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& elbowTarget, bool setOrientation, const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::setTarget(const Eigen::Matrix4f& tcpTarget,
+ const Eigen::Vector3f& elbowTarget,
+ bool setOrientation,
+ const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufTarget.getWriteBuffer();
@@ -388,10 +402,17 @@ namespace armarx
w.setOrientation = setOrientation;
w.updated = true;
_tripBufTarget.commitWrite();
- ARMARX_IMPORTANT << "set new target\n" << tcpTarget << "\nelbow: " << elbowTarget.transpose();
+ ARMARX_IMPORTANT << "set new target\n"
+ << tcpTarget << "\nelbow: " << elbowTarget.transpose();
}
- void NJointCartesianNaturalPositionController::setTargetFeedForward(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& elbowTarget, bool setOrientation, const Eigen::Vector6f& ffVel, const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::setTargetFeedForward(
+ const Eigen::Matrix4f& tcpTarget,
+ const Eigen::Vector3f& elbowTarget,
+ bool setOrientation,
+ const Eigen::Vector6f& ffVel,
+ const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
{
@@ -410,7 +431,9 @@ namespace armarx
_tripBufTarget.commitWrite();
}
- void NJointCartesianNaturalPositionController::setFeedForwardVelocity(const Eigen::Vector6f& vel, const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::setFeedForwardVelocity(const Eigen::Vector6f& vel,
+ const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufFFvel.getWriteBuffer();
@@ -421,7 +444,8 @@ namespace armarx
ARMARX_IMPORTANT << "set new FeedForwardVelocity\n" << vel.transpose();
}
- void NJointCartesianNaturalPositionController::clearFeedForwardVelocity(const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::clearFeedForwardVelocity(const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufFFvel.getWriteBuffer();
@@ -431,7 +455,9 @@ namespace armarx
_tripBufFFvel.commitWrite();
ARMARX_IMPORTANT << "cleared FeedForwardVelocity";
}
- void NJointCartesianNaturalPositionController::setNullVelocity()
+
+ void
+ NJointCartesianNaturalPositionController::setNullVelocity()
{
for (auto ptr : _rtVelTargets)
{
@@ -445,26 +471,34 @@ namespace armarx
// return _publishIsAtForceLimit;
//}
- void NJointCartesianNaturalPositionController::setVisualizationRobotGlobalPose(const Eigen::Matrix4f& p, const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::setVisualizationRobotGlobalPose(
+ const Eigen::Matrix4f& p,
+ const Ice::Current&)
{
std::lock_guard g{_tripFakeRobotGPWriteMutex};
_tripFakeRobotGP.getWriteBuffer() = p;
_tripFakeRobotGP.commitWrite();
}
- void NJointCartesianNaturalPositionController::resetVisualizationRobotGlobalPose(const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::resetVisualizationRobotGlobalPose(const Ice::Current&)
{
std::lock_guard g{_tripFakeRobotGPWriteMutex};
_tripFakeRobotGP.getWriteBuffer()(0, 0) = std::nanf("");
_tripFakeRobotGP.commitWrite();
}
- void NJointCartesianNaturalPositionController::stopMovement(const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::stopMovement(const Ice::Current&)
{
_stopNowRequested = true;
}
- void NJointCartesianNaturalPositionController::setNullspaceTarget(const Ice::FloatSeq& nullspaceTarget, const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::setNullspaceTarget(
+ const Ice::FloatSeq& nullspaceTarget,
+ const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufNullspaceTarget.getWriteBuffer();
@@ -475,7 +509,8 @@ namespace armarx
ARMARX_IMPORTANT << "set new nullspaceTarget\n" << nullspaceTarget;
}
- void NJointCartesianNaturalPositionController::clearNullspaceTarget(const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::clearNullspaceTarget(const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufNullspaceTarget.getWriteBuffer();
@@ -485,12 +520,15 @@ namespace armarx
ARMARX_IMPORTANT << "reset nullspaceTarget";
}
- void NJointCartesianNaturalPositionController::setNullspaceControlEnabled(bool enabled, const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::setNullspaceControlEnabled(bool enabled,
+ const Ice::Current&)
{
_nullspaceControlEnabled = enabled;
}
- FTSensorValue NJointCartesianNaturalPositionController::frost(const FTval& ft)
+ FTSensorValue
+ NJointCartesianNaturalPositionController::frost(const FTval& ft)
{
FTSensorValue r;
r.force = ft.force;
@@ -498,19 +536,23 @@ namespace armarx
return r;
}
- FTSensorValue NJointCartesianNaturalPositionController::getCurrentFTValue(const Ice::Current&)
+ FTSensorValue
+ NJointCartesianNaturalPositionController::getCurrentFTValue(const Ice::Current&)
{
std::lock_guard g{_tripRt2NonRtMutex};
return frost(_tripRt2NonRt.getUpToDateReadBuffer().currentFT);
}
- FTSensorValue NJointCartesianNaturalPositionController::getAverageFTValue(const Ice::Current&)
+ FTSensorValue
+ NJointCartesianNaturalPositionController::getAverageFTValue(const Ice::Current&)
{
std::lock_guard g{_tripRt2NonRtMutex};
return frost(_tripRt2NonRt.getUpToDateReadBuffer().averageFT);
}
- void NJointCartesianNaturalPositionController::setFTOffset(const FTSensorValue& offset, const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::setFTOffset(const FTSensorValue& offset,
+ const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufFToffset.getWriteBuffer();
@@ -518,9 +560,13 @@ namespace armarx
w.torque = offset.torque;
w.updated = true;
_tripBufFToffset.commitWrite();
- ARMARX_IMPORTANT << "set FT offset:\n" << offset.force.transpose() << "\n" << offset.torque.transpose();
+ ARMARX_IMPORTANT << "set FT offset:\n"
+ << offset.force.transpose() << "\n"
+ << offset.torque.transpose();
}
- void NJointCartesianNaturalPositionController::resetFTOffset(const Ice::Current&)
+
+ void
+ NJointCartesianNaturalPositionController::resetFTOffset(const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufFToffset.getWriteBuffer();
@@ -531,9 +577,10 @@ namespace armarx
ARMARX_IMPORTANT << "reset FT offset";
}
-
-
- void NJointCartesianNaturalPositionController::setFTLimit(float force, float torque, const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::setFTLimit(float force,
+ float torque,
+ const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufFTlimit.getWriteBuffer();
@@ -544,7 +591,8 @@ namespace armarx
ARMARX_IMPORTANT << "set FT limit:" << force << " " << torque;
}
- void NJointCartesianNaturalPositionController::clearFTLimit(const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::clearFTLimit(const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufFTlimit.getWriteBuffer();
@@ -555,7 +603,9 @@ namespace armarx
ARMARX_IMPORTANT << "reset FT limit";
}
- void NJointCartesianNaturalPositionController::setFakeFTValue(const FTSensorValue& ftValue, const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::setFakeFTValue(const FTSensorValue& ftValue,
+ const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufFTfake.getWriteBuffer();
@@ -564,11 +614,13 @@ namespace armarx
w.use = true;
w.updated = true;
_tripBufFTfake.commitWrite();
- ARMARX_IMPORTANT << "set fake FT value:\n" << ftValue.force.transpose() << "\n" << ftValue.torque.transpose();
-
+ ARMARX_IMPORTANT << "set fake FT value:\n"
+ << ftValue.force.transpose() << "\n"
+ << ftValue.torque.transpose();
}
- void NJointCartesianNaturalPositionController::clearFakeFTValue(const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::clearFakeFTValue(const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufFTfake.getWriteBuffer();
@@ -580,16 +632,17 @@ namespace armarx
ARMARX_IMPORTANT << "cleared fake FT value";
}
- bool NJointCartesianNaturalPositionController::isAtForceLimit(const Ice::Current&)
+ bool
+ NJointCartesianNaturalPositionController::isAtForceLimit(const Ice::Current&)
{
ARMARX_CHECK_NOT_NULL(_rtForceSensor);
return _publishIsAtForceLimit;
}
- void NJointCartesianNaturalPositionController::onPublish(
- const SensorAndControl&,
- const DebugDrawerInterfacePrx& drawer,
- const DebugObserverInterfacePrx& obs)
+ void
+ NJointCartesianNaturalPositionController::onPublish(const SensorAndControl&,
+ const DebugDrawerInterfacePrx& drawer,
+ const DebugObserverInterfacePrx& obs)
{
const float errorPos = _publish.errorPos;
const float errorOri = _publish.errorOri;
@@ -619,10 +672,8 @@ namespace armarx
obs->setDebugChannel(getInstanceName(), datafields);
}
- ARMARX_INFO << deactivateSpam(1) << std::setprecision(4)
- << "perror " << errorPos << " / " << errorPosMax
- << ", oerror " << errorOri << " / " << errorOriMax
- << ')';
+ ARMARX_INFO << deactivateSpam(1) << std::setprecision(4) << "perror " << errorPos << " / "
+ << errorPosMax << ", oerror " << errorOri << " / " << errorOriMax << ')';
{
std::lock_guard g{_tripRt2NonRtMutex};
@@ -639,16 +690,18 @@ namespace armarx
const Eigen::Matrix4f gelbtarg = gp * buf.elbTarg;
drawer->setPoseVisu(getName(), "tcp", new Pose(gtcp));
drawer->setPoseVisu(getName(), "tcptarg", new Pose(gtcptarg));
- drawer->setLineVisu(
- getName(), "tcp2targ",
- new Vector3(Eigen::Vector3f{gtcp.topRightCorner<3, 1>()}),
- new Vector3(Eigen::Vector3f{gtcptarg.topRightCorner<3, 1>()}),
- 3, {0, 0, 1, 1});
- drawer->setLineVisu(
- getName(), "elb2targ",
- new Vector3(Eigen::Vector3f{gelb.topRightCorner<3, 1>()}),
- new Vector3(Eigen::Vector3f{gelbtarg.topRightCorner<3, 1>()}),
- 3, {0, 0, 1, 1});
+ drawer->setLineVisu(getName(),
+ "tcp2targ",
+ new Vector3(Eigen::Vector3f{gtcp.topRightCorner<3, 1>()}),
+ new Vector3(Eigen::Vector3f{gtcptarg.topRightCorner<3, 1>()}),
+ 3,
+ {0, 0, 1, 1});
+ drawer->setLineVisu(getName(),
+ "elb2targ",
+ new Vector3(Eigen::Vector3f{gelb.topRightCorner<3, 1>()}),
+ new Vector3(Eigen::Vector3f{gelbtarg.topRightCorner<3, 1>()}),
+ 3,
+ {0, 0, 1, 1});
}
else
{
@@ -671,8 +724,4 @@ namespace armarx
//}
}
}
-}
-
-
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
index 41276169749fe1156623f14bf4a8589c500b2266..fdf9bbf5df49848fe072c820111c3835ee353960 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
@@ -1,12 +1,12 @@
#pragma once
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
-#include <RobotAPI/libraries/core/CartesianNaturalPositionController.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
-#include <VirtualRobot/VirtualRobot.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.h>
+#include <RobotAPI/libraries/core/CartesianNaturalPositionController.h>
namespace armarx
{
@@ -32,17 +32,28 @@ namespace armarx
std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override;
// NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
- void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
+ void onPublish(const SensorAndControl&,
+ const DebugDrawerInterfacePrx&,
+ const DebugObserverInterfacePrx&) override;
public:
//bool hasReachedTarget(const Ice::Current& = Ice::emptyCurrent) override;
//bool hasReachedForceLimit(const Ice::Current& = Ice::emptyCurrent) override;
- void setConfig(const CartesianNaturalPositionControllerConfig& cfg, const Ice::Current& = Ice::emptyCurrent) override;
- void setTarget(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& elbowTarget, bool setOrientation, const Ice::Current& = Ice::emptyCurrent) override;
- void setTargetFeedForward(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& elbowTarget, bool setOrientation, const Eigen::Vector6f& ffVel, const Ice::Current&) override;
+ void setConfig(const CartesianNaturalPositionControllerConfig& cfg,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setTarget(const Eigen::Matrix4f& tcpTarget,
+ const Eigen::Vector3f& elbowTarget,
+ bool setOrientation,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setTargetFeedForward(const Eigen::Matrix4f& tcpTarget,
+ const Eigen::Vector3f& elbowTarget,
+ bool setOrientation,
+ const Eigen::Vector6f& ffVel,
+ const Ice::Current&) override;
void setFeedForwardVelocity(const Eigen::Vector6f& vel, const Ice::Current&) override;
void clearFeedForwardVelocity(const Ice::Current&) override;
void setNullspaceTarget(const Ice::FloatSeq& nullspaceTarget, const Ice::Current&) override;
@@ -61,7 +72,8 @@ namespace armarx
void stopMovement(const Ice::Current& = Ice::emptyCurrent) override;
- void setVisualizationRobotGlobalPose(const Eigen::Matrix4f& p, const Ice::Current& = Ice::emptyCurrent) override;
+ void setVisualizationRobotGlobalPose(const Eigen::Matrix4f& p,
+ const Ice::Current& = Ice::emptyCurrent) override;
void resetVisualizationRobotGlobalPose(const Ice::Current& = Ice::emptyCurrent) override;
protected:
@@ -70,7 +82,6 @@ namespace armarx
//structs
private:
-
struct TB_Target
{
Eigen::Matrix4f tcpTarget;
@@ -78,45 +89,52 @@ namespace armarx
bool setOrientation;
bool updated = false;
};
+
struct TB_FFvel
{
Eigen::Vector6f feedForwardVelocity = Eigen::Vector6f::Zero();
bool use = false;
bool updated = false;
};
+
struct TB_Cfg
{
CartesianNaturalPositionControllerConfig cfg;
bool updated = false;
};
+
struct TB_NT
{
std::vector<float> nullspaceTarget;
bool clearRequested = false;
bool updated = false;
};
+
struct TB_FT
{
- Eigen::Vector3f force = Eigen::Vector3f::Zero();
+ Eigen::Vector3f force = Eigen::Vector3f::Zero();
Eigen::Vector3f torque = Eigen::Vector3f::Zero();
bool updated = false;
};
+
struct TB_FTlimit
{
float force = -1;
float torque = -1;
bool updated = false;
};
+
struct TB_FTfake
{
- Eigen::Vector3f force = Eigen::Vector3f::Zero();
+ Eigen::Vector3f force = Eigen::Vector3f::Zero();
Eigen::Vector3f torque = Eigen::Vector3f::Zero();
bool use = false;
bool updated = false;
};
+
struct FTval
{
- Eigen::Vector3f force = Eigen::Vector3f::Zero();
+ Eigen::Vector3f force = Eigen::Vector3f::Zero();
Eigen::Vector3f torque = Eigen::Vector3f::Zero();
};
@@ -155,72 +173,69 @@ namespace armarx
using Ctrl = CartesianNaturalPositionController;
//rt data
- VirtualRobot::RobotPtr _rtRobot;
- VirtualRobot::RobotNodeSetPtr _rtRns;
- VirtualRobot::RobotNodePtr _rtTcp;
- VirtualRobot::RobotNodePtr _rtElbow;
- VirtualRobot::RobotNodePtr _rtFT;
- VirtualRobot::RobotNodePtr _rtRobotRoot;
+ VirtualRobot::RobotPtr _rtRobot;
+ VirtualRobot::RobotNodeSetPtr _rtRns;
+ VirtualRobot::RobotNodePtr _rtTcp;
+ VirtualRobot::RobotNodePtr _rtElbow;
+ VirtualRobot::RobotNodePtr _rtFT;
+ VirtualRobot::RobotNodePtr _rtRobotRoot;
- std::unique_ptr<Ctrl> _rtPosController;
- bool _rtSetOrientation = true;
- Eigen::VectorXf _rtJointVelocityFeedbackBuffer;
- TB_FFvel _rtFFvel;
- int _rtFFvelMaxAgeMS;
- long _rtFFvelLastUpdateMS = 0;
+ std::unique_ptr<Ctrl> _rtPosController;
+ bool _rtSetOrientation = true;
+ Eigen::VectorXf _rtJointVelocityFeedbackBuffer;
+ TB_FFvel _rtFFvel;
+ int _rtFFvelMaxAgeMS;
+ long _rtFFvelLastUpdateMS = 0;
- std::vector<const float*> _rtVelSensors;
- std::vector<float*> _rtVelTargets;
+ std::vector<const float*> _rtVelSensors;
+ std::vector<float*> _rtVelTargets;
- const Eigen::Vector3f* _rtForceSensor = nullptr;
- const Eigen::Vector3f* _rtTorqueSensor = nullptr;
+ const Eigen::Vector3f* _rtForceSensor = nullptr;
+ const Eigen::Vector3f* _rtTorqueSensor = nullptr;
- std::vector<FTval> _rtFTHistory;
- size_t _rtFTHistoryIndex = 0;
+ std::vector<FTval> _rtFTHistory;
+ size_t _rtFTHistoryIndex = 0;
TB_FT _rtFTOffset;
- TB_FTlimit _rtFTlimit;
- TB_FTfake _rtFTfake;
+ TB_FTlimit _rtFTlimit;
+ TB_FTfake _rtFTfake;
//Eigen::Vector3f _rtForceOffset;
//float _rtForceThreshold = -1;
- bool _rtStopConditionReached = false;
+ bool _rtStopConditionReached = false;
- std::atomic_bool _nullspaceControlEnabled{true};
+ std::atomic_bool _nullspaceControlEnabled{true};
//flags
//std::atomic_bool _publishIsAtTarget{false};
- std::atomic_bool _publishIsAtForceLimit{false};
+ std::atomic_bool _publishIsAtForceLimit{false};
//std::atomic_bool _setFTOffset{false};
- std::atomic_bool _stopNowRequested{false};
+ std::atomic_bool _stopNowRequested{false};
//buffers
- mutable std::recursive_mutex _mutexSetTripBuf;
+ mutable std::recursive_mutex _mutexSetTripBuf;
//TripleBuffer<CtrlData> _tripBufPosCtrl;
- TripleBuffer<TB_Target> _tripBufTarget;
- TripleBuffer<TB_NT> _tripBufNullspaceTarget;
- TripleBuffer<TB_FFvel> _tripBufFFvel;
- TripleBuffer<TB_Cfg> _tripBufCfg;
- TripleBuffer<TB_FT> _tripBufFToffset;
- TripleBuffer<TB_FTlimit> _tripBufFTlimit;
- TripleBuffer<TB_FTfake> _tripBufFTfake;
+ TripleBuffer<TB_Target> _tripBufTarget;
+ TripleBuffer<TB_NT> _tripBufNullspaceTarget;
+ TripleBuffer<TB_FFvel> _tripBufFFvel;
+ TripleBuffer<TB_Cfg> _tripBufCfg;
+ TripleBuffer<TB_FT> _tripBufFToffset;
+ TripleBuffer<TB_FTlimit> _tripBufFTlimit;
+ TripleBuffer<TB_FTfake> _tripBufFTfake;
- mutable std::recursive_mutex _tripRt2NonRtMutex;
- TripleBuffer<RtToNonRtData> _tripRt2NonRt;
+ mutable std::recursive_mutex _tripRt2NonRtMutex;
+ TripleBuffer<RtToNonRtData> _tripRt2NonRt;
- mutable std::recursive_mutex _tripFakeRobotGPWriteMutex;
- TripleBuffer<Eigen::Matrix4f> _tripFakeRobotGP;
+ mutable std::recursive_mutex _tripFakeRobotGPWriteMutex;
+ TripleBuffer<Eigen::Matrix4f> _tripFakeRobotGP;
//publish data
- PublishData _publish;
+ PublishData _publish;
//std::atomic<float> _publishForceThreshold{0};
//std::atomic<float> _publishForceCurrent{0};
//std::atomic_bool _publishForceThresholdInRobotRootZ{0};
-
-
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp
index 11b3246034d8b9f7ae38f6d60244e0b1f9f5f9cb..ec93568c8452b7268c5ad2212ddb9df954e8b9ce 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp
@@ -22,11 +22,12 @@
* GNU General Public License
*/
#include "NJointCartesianTorqueController.h"
-#include "../RobotUnit.h"
+
+#include <VirtualRobot/RobotNodeSet.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-#include <VirtualRobot/RobotNodeSet.h>
+#include "../RobotUnit.h"
#define DEFAULT_TCP_STRING "default TCP"
@@ -34,14 +35,19 @@
namespace armarx
{
- NJointControllerRegistration<NJointCartesianTorqueController> registrationControllerNJointCartesianTorqueController("NJointCartesianTorqueController");
+ NJointControllerRegistration<NJointCartesianTorqueController>
+ registrationControllerNJointCartesianTorqueController("NJointCartesianTorqueController");
- std::string NJointCartesianTorqueController::getClassName(const Ice::Current&) const
+ std::string
+ NJointCartesianTorqueController::getClassName(const Ice::Current&) const
{
return "NJointCartesianTorqueController";
}
- NJointCartesianTorqueController::NJointCartesianTorqueController(RobotUnit*, NJointCartesianTorqueControllerConfigPtr config, const VirtualRobot::RobotPtr& r)
+ NJointCartesianTorqueController::NJointCartesianTorqueController(
+ RobotUnit*,
+ NJointCartesianTorqueControllerConfigPtr config,
+ const VirtualRobot::RobotPtr& r)
{
ARMARX_CHECK_EXPRESSION(!config->nodeSetName.empty());
useSynchronizedRtRobot();
@@ -68,12 +74,15 @@ namespace armarx
gravityTorqueSensors.push_back(gravityTorqueSensor);*/
};
- tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING) ? rns->getTCP() : rtGetRobot()->getRobotNode(config->tcpName);
+ tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING)
+ ? rns->getTCP()
+ : rtGetRobot()->getRobotNode(config->tcpName);
ARMARX_CHECK_EXPRESSION(tcp) << config->tcpName;
nodeSetName = config->nodeSetName;
- ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ ik.reset(new VirtualRobot::DifferentialIK(
+ rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
/*NJointCartesianTorqueControllerControlData initData;
@@ -83,18 +92,22 @@ namespace armarx
reinitTripleBuffer(initData);*/
}
-
- void NJointCartesianTorqueController::rtPreActivateController()
+ void
+ NJointCartesianTorqueController::rtPreActivateController()
{
}
- void NJointCartesianTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ NJointCartesianTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
Eigen::VectorXf cartesianFT(6);
- cartesianFT << rtGetControlStruct().forceX / 1000, rtGetControlStruct().forceY / 1000, rtGetControlStruct().forceZ / 1000,
- rtGetControlStruct().torqueX, rtGetControlStruct().torqueY, rtGetControlStruct().torqueZ;
+ cartesianFT << rtGetControlStruct().forceX / 1000, rtGetControlStruct().forceY / 1000,
+ rtGetControlStruct().forceZ / 1000, rtGetControlStruct().torqueX,
+ rtGetControlStruct().torqueY, rtGetControlStruct().torqueZ;
- Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
+ Eigen::MatrixXf jacobi =
+ ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
Eigen::MatrixXf jacobiT = jacobi.transpose();
@@ -108,12 +121,12 @@ namespace armarx
}
}
-
- WidgetDescription::HBoxLayoutPtr NJointCartesianTorqueController::createSliders()
+ WidgetDescription::HBoxLayoutPtr
+ NJointCartesianTorqueController::createSliders()
{
using namespace armarx::WidgetDescription;
HBoxLayoutPtr layout = new HBoxLayout;
- auto addSlider = [&](const std::string & label, float min, float max, float defaultValue)
+ auto addSlider = [&](const std::string& label, float min, float max, float defaultValue)
{
layout->children.emplace_back(new Label(false, label));
FloatSliderPtr floatSlider = new FloatSlider;
@@ -157,7 +170,11 @@ namespace armarx
return layout;
}*/
- WidgetDescription::WidgetPtr NJointCartesianTorqueController::GenerateConfigDescription(const VirtualRobot::RobotPtr& robot, const std::map<std::string, ConstControlDevicePtr>& controlDevices, const std::map<std::string, ConstSensorDevicePtr>&)
+ WidgetDescription::WidgetPtr
+ NJointCartesianTorqueController::GenerateConfigDescription(
+ const VirtualRobot::RobotPtr& robot,
+ const std::map<std::string, ConstControlDevicePtr>& controlDevices,
+ const std::map<std::string, ConstSensorDevicePtr>&)
{
using namespace armarx::WidgetDescription;
HBoxLayoutPtr layout = new HBoxLayout;
@@ -198,37 +215,41 @@ namespace armarx
return layout;
}
- NJointCartesianTorqueControllerConfigPtr NJointCartesianTorqueController::GenerateConfigFromVariants(const StringVariantBaseMap& values)
+ NJointCartesianTorqueControllerConfigPtr
+ NJointCartesianTorqueController::GenerateConfigFromVariants(const StringVariantBaseMap& values)
{
- return new NJointCartesianTorqueControllerConfig(values.at("nodeSetName")->getString(), values.at("tcpName")->getString());
+ return new NJointCartesianTorqueControllerConfig(values.at("nodeSetName")->getString(),
+ values.at("tcpName")->getString());
}
- std::string NJointCartesianTorqueController::getNodeSetName() const
+ std::string
+ NJointCartesianTorqueController::getNodeSetName() const
{
return nodeSetName;
}
- void NJointCartesianTorqueController::rtPostDeactivateController()
+ void
+ NJointCartesianTorqueController::rtPostDeactivateController()
{
-
}
- WidgetDescription::StringWidgetDictionary NJointCartesianTorqueController::getFunctionDescriptions(const Ice::Current&) const
+ WidgetDescription::StringWidgetDictionary
+ NJointCartesianTorqueController::getFunctionDescriptions(const Ice::Current&) const
{
using namespace armarx::WidgetDescription;
HBoxLayoutPtr layout = createSliders();
- return
- {
- {"ControllerTarget", layout}
- };
+ return {{"ControllerTarget", layout}};
}
- void NJointCartesianTorqueController::callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&)
+ void
+ NJointCartesianTorqueController::callDescribedFunction(const std::string& name,
+ const StringVariantBaseMap& valueMap,
+ const Ice::Current&)
{
if (name == "ControllerTarget")
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().forceX = valueMap.at("forceX")->getFloat();
getWriterControlStruct().forceY = valueMap.at("forceY")->getFloat();
getWriterControlStruct().forceZ = valueMap.at("forceZ")->getFloat();
@@ -244,11 +265,16 @@ namespace armarx
}
} // namespace armarx
-
-
-void armarx::NJointCartesianTorqueController::setControllerTarget(float forceX, float forceY, float forceZ, float torqueX, float torqueY, float torqueZ, const Ice::Current&)
+void
+armarx::NJointCartesianTorqueController::setControllerTarget(float forceX,
+ float forceY,
+ float forceZ,
+ float torqueX,
+ float torqueY,
+ float torqueZ,
+ const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().forceX = forceX;
getWriterControlStruct().forceY = forceY;
getWriterControlStruct().forceZ = forceZ;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
index 2bb10f999e6cf7c9a0a873ce1316e5148550732f..91326e4935517bb6b05457e2843615647a7fa05c 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
@@ -24,12 +24,14 @@
#pragma once
-#include "NJointControllerWithTripleBuffer.h"
+#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Robot.h>
+
+#include <RobotAPI/interface/units/RobotUnit/NJointCartesianTorqueController.h>
+
#include "../ControlTargets/ControlTarget1DoFActuator.h"
#include "../SensorValues/SensorValue1DoFActuator.h"
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointCartesianTorqueController.h>
+#include "NJointControllerWithTripleBuffer.h"
namespace armarx
{
@@ -52,6 +54,7 @@ namespace armarx
};*/
TYPEDEF_PTRS_HANDLE(NJointCartesianTorqueControllerControlData);
+
class NJointCartesianTorqueControllerControlData
{
public:
@@ -65,7 +68,6 @@ namespace armarx
//VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All;
};
-
/**
* @brief The NJointCartesianTorqueController class
* @ingroup Library-RobotUnit-NJointControllers
@@ -76,25 +78,31 @@ namespace armarx
{
public:
using ConfigPtrT = NJointCartesianTorqueControllerConfigPtr;
- NJointCartesianTorqueController(RobotUnit* prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+ NJointCartesianTorqueController(RobotUnit* prov,
+ const NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&);
// NJointControllerInterface interface
std::string getClassName(const Ice::Current&) const override;
- WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current&) const override;
- void callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&) override;
+ WidgetDescription::StringWidgetDictionary
+ getFunctionDescriptions(const Ice::Current&) const override;
+ void callDescribedFunction(const std::string& name,
+ const StringVariantBaseMap& valueMap,
+ const Ice::Current&) override;
// NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
- static WidgetDescription::WidgetPtr GenerateConfigDescription(
- const VirtualRobot::RobotPtr&,
- const std::map<std::string, ConstControlDevicePtr>&,
- const std::map<std::string, ConstSensorDevicePtr>&);
-
- static NJointCartesianTorqueControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
- NJointCartesianTorqueController(
- RobotUnit* prov,
- NJointCartesianTorqueControllerConfigPtr config,
- const VirtualRobot::RobotPtr& r);
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
+ static WidgetDescription::WidgetPtr
+ GenerateConfigDescription(const VirtualRobot::RobotPtr&,
+ const std::map<std::string, ConstControlDevicePtr>&,
+ const std::map<std::string, ConstSensorDevicePtr>&);
+
+ static NJointCartesianTorqueControllerConfigPtr
+ GenerateConfigFromVariants(const StringVariantBaseMap& values);
+ NJointCartesianTorqueController(RobotUnit* prov,
+ NJointCartesianTorqueControllerConfigPtr config,
+ const VirtualRobot::RobotPtr& r);
std::string getNodeSetName() const;
static ::armarx::WidgetDescription::HBoxLayoutPtr createSliders();
@@ -103,6 +111,7 @@ namespace armarx
protected:
void rtPreActivateController() override;
void rtPostDeactivateController() override;
+
private:
std::vector<ControlTarget1DoFActuatorTorque*> targets;
//std::vector<const SensorValue1DoFActuatorPosition*> sensors;
@@ -117,8 +126,13 @@ namespace armarx
// NJointCartesianTorqueControllerInterface interface
public:
- void setControllerTarget(float forceX, float forceY, float forceZ, float torqueX, float torqueY, float torqueZ, const Ice::Current&) override;
+ void setControllerTarget(float forceX,
+ float forceY,
+ float forceZ,
+ float torqueX,
+ float torqueY,
+ float torqueZ,
+ const Ice::Current&) override;
};
} // namespace armarx
-
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
index 2b55cc15c11be833e581c36eafbdf31cdffec55c..486eaa77564de408fcd7c57da71eb1c73e6e93f1 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
@@ -24,24 +24,30 @@
#include "NJointCartesianVelocityController.h"
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-
-#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#define DEFAULT_TCP_STRING "default TCP"
namespace armarx
{
- NJointControllerRegistration<NJointCartesianVelocityController> registrationControllerNJointCartesianVelocityController("NJointCartesianVelocityController");
+ NJointControllerRegistration<NJointCartesianVelocityController>
+ registrationControllerNJointCartesianVelocityController(
+ "NJointCartesianVelocityController");
- std::string NJointCartesianVelocityController::getClassName(const Ice::Current&) const
+ std::string
+ NJointCartesianVelocityController::getClassName(const Ice::Current&) const
{
return "NJointCartesianVelocityController";
}
- NJointCartesianVelocityController::NJointCartesianVelocityController(RobotUnit* robotUnit, const NJointCartesianVelocityControllerConfigPtr& config, const VirtualRobot::RobotPtr& r)
+ NJointCartesianVelocityController::NJointCartesianVelocityController(
+ RobotUnit* robotUnit,
+ const NJointCartesianVelocityControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr& r)
{
useSynchronizedRtRobot();
ARMARX_CHECK_EXPRESSION(!config->nodeSetName.empty());
@@ -54,8 +60,10 @@ namespace armarx
const SensorValueBase* sv = useSensorValue(jointName);
targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
- const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
- const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
+ const SensorValue1DoFActuatorTorque* torqueSensor =
+ sv->asA<SensorValue1DoFActuatorTorque>();
+ const SensorValue1DoFGravityTorque* gravityTorqueSensor =
+ sv->asA<SensorValue1DoFGravityTorque>();
if (!torqueSensor)
{
ARMARX_WARNING << "No Torque sensor available for " << jointName;
@@ -68,7 +76,10 @@ namespace armarx
gravityTorqueSensors.push_back(gravityTorqueSensor);
};
- VirtualRobot::RobotNodePtr tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING) ? rns->getTCP() : rtGetRobot()->getRobotNode(config->tcpName);
+ VirtualRobot::RobotNodePtr tcp =
+ (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING)
+ ? rns->getTCP()
+ : rtGetRobot()->getRobotNode(config->tcpName);
ARMARX_CHECK_EXPRESSION(tcp) << config->tcpName;
tcpController.reset(new CartesianVelocityController(rns, tcp));
@@ -84,12 +95,14 @@ namespace armarx
reinitTripleBuffer(initData);
}
-
- void NJointCartesianVelocityController::rtPreActivateController()
+ void
+ NJointCartesianVelocityController::rtPreActivateController()
{
}
- void NJointCartesianVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ NJointCartesianVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
auto mode = rtGetControlStruct().mode;
@@ -97,7 +110,9 @@ namespace armarx
if (mode == VirtualRobot::IKSolver::All)
{
x.resize(6);
- x << rtGetControlStruct().xVel, rtGetControlStruct().yVel, rtGetControlStruct().zVel, rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel, rtGetControlStruct().yawVel;
+ x << rtGetControlStruct().xVel, rtGetControlStruct().yVel, rtGetControlStruct().zVel,
+ rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel,
+ rtGetControlStruct().yawVel;
}
else if (mode == VirtualRobot::IKSolver::Position)
{
@@ -107,7 +122,8 @@ namespace armarx
else if (mode == VirtualRobot::IKSolver::Orientation)
{
x.resize(3);
- x << rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel, rtGetControlStruct().yawVel;
+ x << rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel,
+ rtGetControlStruct().yawVel;
}
else
{
@@ -124,11 +140,14 @@ namespace armarx
for (size_t i = 0; i < tcpController->rns->getSize(); i++)
{
jnv(i) += rtGetControlStruct().nullspaceJointVelocities.at(i);
- if (torqueSensors.at(i) && gravityTorqueSensors.at(i) && rtGetControlStruct().torqueKp.at(i) != 0)
+ if (torqueSensors.at(i) && gravityTorqueSensors.at(i) &&
+ rtGetControlStruct().torqueKp.at(i) != 0)
{
torquePIDs.at(i).Kp = rtGetControlStruct().torqueKp.at(i);
torquePIDs.at(i).Kd = rtGetControlStruct().torqueKd.at(i);
- jnv(i) += torquePIDs.at(i).update(timeSinceLastIteration.toSecondsDouble(), torqueSensors.at(i)->torque - gravityTorqueSensors.at(i)->gravityTorque);
+ jnv(i) += torquePIDs.at(i).update(timeSinceLastIteration.toSecondsDouble(),
+ torqueSensors.at(i)->torque -
+ gravityTorqueSensors.at(i)->gravityTorque);
//jnv(i) += rtGetControlStruct().torqueKp.at(i) * (torqueSensors.at(i)->torque - gravityTorqueSensors.at(i)->gravityTorque);
}
else
@@ -144,12 +163,12 @@ namespace armarx
}
}
-
- ::armarx::WidgetDescription::WidgetSeq NJointCartesianVelocityController::createSliders()
+ ::armarx::WidgetDescription::WidgetSeq
+ NJointCartesianVelocityController::createSliders()
{
using namespace armarx::WidgetDescription;
::armarx::WidgetDescription::WidgetSeq widgets;
- auto addSlider = [&](const std::string & label, float limit)
+ auto addSlider = [&](const std::string& label, float limit)
{
widgets.emplace_back(new Label(false, label));
{
@@ -172,11 +191,14 @@ namespace armarx
return widgets;
}
- WidgetDescription::HBoxLayoutPtr NJointCartesianVelocityController::createJointSlidersLayout(float min, float max, float defaultValue) const
+ WidgetDescription::HBoxLayoutPtr
+ NJointCartesianVelocityController::createJointSlidersLayout(float min,
+ float max,
+ float defaultValue) const
{
using namespace armarx::WidgetDescription;
HBoxLayoutPtr layout = new HBoxLayout;
- auto addSlider = [&](const std::string & label)
+ auto addSlider = [&](const std::string& label)
{
layout->children.emplace_back(new Label(false, label));
FloatSliderPtr floatSlider = new FloatSlider;
@@ -193,10 +215,13 @@ namespace armarx
}
return layout;
-
}
- WidgetDescription::WidgetPtr NJointCartesianVelocityController::GenerateConfigDescription(const VirtualRobot::RobotPtr& robot, const std::map<std::string, ConstControlDevicePtr>& controlDevices, const std::map<std::string, ConstSensorDevicePtr>&)
+ WidgetDescription::WidgetPtr
+ NJointCartesianVelocityController::GenerateConfigDescription(
+ const VirtualRobot::RobotPtr& robot,
+ const std::map<std::string, ConstControlDevicePtr>& controlDevices,
+ const std::map<std::string, ConstSensorDevicePtr>&)
{
using namespace armarx::WidgetDescription;
HBoxLayoutPtr layout = new HBoxLayout;
@@ -241,13 +266,18 @@ namespace armarx
return layout;
}
- NJointCartesianVelocityControllerConfigPtr NJointCartesianVelocityController::GenerateConfigFromVariants(const StringVariantBaseMap& values)
+ NJointCartesianVelocityControllerConfigPtr
+ NJointCartesianVelocityController::GenerateConfigFromVariants(
+ const StringVariantBaseMap& values)
{
- return new NJointCartesianVelocityControllerConfig(values.at("nodeSetName")->getString(), values.at("tcpName")->getString(),
- IceModeFromString(values.at("mode")->getString()));
+ return new NJointCartesianVelocityControllerConfig(
+ values.at("nodeSetName")->getString(),
+ values.at("tcpName")->getString(),
+ IceModeFromString(values.at("mode")->getString()));
}
- VirtualRobot::IKSolver::CartesianSelection NJointCartesianVelocityController::ModeFromString(const std::string mode)
+ VirtualRobot::IKSolver::CartesianSelection
+ NJointCartesianVelocityController::ModeFromString(const std::string mode)
{
//ARMARX_IMPORTANT_S << "the mode is " << mode;
if (mode == "Position")
@@ -266,7 +296,8 @@ namespace armarx
return (VirtualRobot::IKSolver::CartesianSelection)0;
}
- NJointCartesianVelocityControllerMode::CartesianSelection NJointCartesianVelocityController::IceModeFromString(const std::string mode)
+ NJointCartesianVelocityControllerMode::CartesianSelection
+ NJointCartesianVelocityController::IceModeFromString(const std::string mode)
{
if (mode == "Position")
{
@@ -284,7 +315,9 @@ namespace armarx
return (NJointCartesianVelocityControllerMode::CartesianSelection)0;
}
- VirtualRobot::IKSolver::CartesianSelection NJointCartesianVelocityController::ModeFromIce(const NJointCartesianVelocityControllerMode::CartesianSelection mode)
+ VirtualRobot::IKSolver::CartesianSelection
+ NJointCartesianVelocityController::ModeFromIce(
+ const NJointCartesianVelocityControllerMode::CartesianSelection mode)
{
if (mode == NJointCartesianVelocityControllerMode::CartesianSelection::ePosition)
{
@@ -302,10 +335,17 @@ namespace armarx
return (VirtualRobot::IKSolver::CartesianSelection)0;
}
-
- void NJointCartesianVelocityController::setVelocities(float xVel, float yVel, float zVel, float rollVel, float pitchVel, float yawVel, VirtualRobot::IKSolver::CartesianSelection mode)
+ void
+ NJointCartesianVelocityController::setVelocities(
+ float xVel,
+ float yVel,
+ float zVel,
+ float rollVel,
+ float pitchVel,
+ float yawVel,
+ VirtualRobot::IKSolver::CartesianSelection mode)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().xVel = xVel;
getWriterControlStruct().yVel = yVel;
getWriterControlStruct().zVel = zVel;
@@ -316,79 +356,84 @@ namespace armarx
writeControlStruct();
}
- void NJointCartesianVelocityController::setAvoidJointLimitsKp(float kp)
+ void
+ NJointCartesianVelocityController::setAvoidJointLimitsKp(float kp)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().avoidJointLimitsKp = kp;
writeControlStruct();
}
- std::string NJointCartesianVelocityController::getNodeSetName() const
+ std::string
+ NJointCartesianVelocityController::getNodeSetName() const
{
return nodeSetName;
}
- void NJointCartesianVelocityController::rtPostDeactivateController()
+ void
+ NJointCartesianVelocityController::rtPostDeactivateController()
{
-
}
- WidgetDescription::StringWidgetDictionary NJointCartesianVelocityController::getFunctionDescriptions(const Ice::Current&) const
+ WidgetDescription::StringWidgetDictionary
+ NJointCartesianVelocityController::getFunctionDescriptions(const Ice::Current&) const
{
using namespace armarx::WidgetDescription;
HBoxLayoutPtr layout = new HBoxLayout;
auto sliders = createSliders();
- layout->children.insert(layout->children.end(),
- sliders.begin(),
- sliders.end());
+ layout->children.insert(layout->children.end(), sliders.begin(), sliders.end());
- return
- {
- {"ControllerTarget", layout},
- {"TorqueKp", createJointSlidersLayout(-1, 1, 0)},
- {"TorqueKd", createJointSlidersLayout(-1, 1, 0)},
- {"NullspaceJointVelocities", createJointSlidersLayout(-1, 1, 0)}
- };
+ return {{"ControllerTarget", layout},
+ {"TorqueKp", createJointSlidersLayout(-1, 1, 0)},
+ {"TorqueKd", createJointSlidersLayout(-1, 1, 0)},
+ {"NullspaceJointVelocities", createJointSlidersLayout(-1, 1, 0)}};
}
- void NJointCartesianVelocityController::callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&)
+ void
+ NJointCartesianVelocityController::callDescribedFunction(const std::string& name,
+ const StringVariantBaseMap& valueMap,
+ const Ice::Current&)
{
if (name == "ControllerTarget")
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().xVel = valueMap.at("x")->getFloat();
getWriterControlStruct().yVel = valueMap.at("y")->getFloat();
getWriterControlStruct().zVel = valueMap.at("z")->getFloat();
getWriterControlStruct().rollVel = valueMap.at("roll")->getFloat();
getWriterControlStruct().pitchVel = valueMap.at("pitch")->getFloat();
getWriterControlStruct().yawVel = valueMap.at("yaw")->getFloat();
- getWriterControlStruct().avoidJointLimitsKp = valueMap.at("avoidJointLimitsKp")->getFloat();
+ getWriterControlStruct().avoidJointLimitsKp =
+ valueMap.at("avoidJointLimitsKp")->getFloat();
writeControlStruct();
}
else if (name == "TorqueKp")
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
for (size_t i = 0; i < tcpController->rns->getSize(); i++)
{
- getWriterControlStruct().torqueKp.at(i) = valueMap.at(tcpController->rns->getNode(i)->getName())->getFloat();
+ getWriterControlStruct().torqueKp.at(i) =
+ valueMap.at(tcpController->rns->getNode(i)->getName())->getFloat();
}
writeControlStruct();
}
else if (name == "TorqueKd")
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
for (size_t i = 0; i < tcpController->rns->getSize(); i++)
{
- getWriterControlStruct().torqueKd.at(i) = valueMap.at(tcpController->rns->getNode(i)->getName())->getFloat();
+ getWriterControlStruct().torqueKd.at(i) =
+ valueMap.at(tcpController->rns->getNode(i)->getName())->getFloat();
}
writeControlStruct();
}
else if (name == "NullspaceJointVelocities")
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
for (size_t i = 0; i < tcpController->rns->getSize(); i++)
{
- getWriterControlStruct().nullspaceJointVelocities.at(i) = valueMap.at(tcpController->rns->getNode(i)->getName())->getFloat();
+ getWriterControlStruct().nullspaceJointVelocities.at(i) =
+ valueMap.at(tcpController->rns->getNode(i)->getName())->getFloat();
}
writeControlStruct();
}
@@ -398,7 +443,8 @@ namespace armarx
}
}
- float SimplePID::update(float dt, float error)
+ float
+ SimplePID::update(float dt, float error)
{
float derivative = (error - lastError) / dt;
float retVal = Kp * error + Kd * derivative;
@@ -408,11 +454,19 @@ namespace armarx
} // namespace armarx
-
-
-void armarx::NJointCartesianVelocityController::setControllerTarget(float x, float y, float z, float roll, float pitch, float yaw, float avoidJointLimitsKp, NJointCartesianVelocityControllerMode::CartesianSelection mode, const Ice::Current&)
+void
+armarx::NJointCartesianVelocityController::setControllerTarget(
+ float x,
+ float y,
+ float z,
+ float roll,
+ float pitch,
+ float yaw,
+ float avoidJointLimitsKp,
+ NJointCartesianVelocityControllerMode::CartesianSelection mode,
+ const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().xVel = x;
getWriterControlStruct().yVel = y;
getWriterControlStruct().zVel = z;
@@ -424,22 +478,29 @@ void armarx::NJointCartesianVelocityController::setControllerTarget(float x, flo
writeControlStruct();
}
-void armarx::NJointCartesianVelocityController::setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&)
+void
+armarx::NJointCartesianVelocityController::setTorqueKp(const StringFloatDictionary& torqueKp,
+ const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
for (size_t i = 0; i < tcpController->rns->getSize(); i++)
{
- getWriterControlStruct().torqueKp.at(i) = torqueKp.at(tcpController->rns->getNode(i)->getName());
+ getWriterControlStruct().torqueKp.at(i) =
+ torqueKp.at(tcpController->rns->getNode(i)->getName());
}
writeControlStruct();
}
-void armarx::NJointCartesianVelocityController::setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&)
+void
+armarx::NJointCartesianVelocityController::setNullspaceJointVelocities(
+ const StringFloatDictionary& nullspaceJointVelocities,
+ const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
for (size_t i = 0; i < tcpController->rns->getSize(); i++)
{
- getWriterControlStruct().nullspaceJointVelocities.at(i) = nullspaceJointVelocities.at(tcpController->rns->getNode(i)->getName());
+ getWriterControlStruct().nullspaceJointVelocities.at(i) =
+ nullspaceJointVelocities.at(tcpController->rns->getNode(i)->getName());
}
writeControlStruct();
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
index aed56406e6f690a8f58013848b44b6deffd6f9c5..35f3a9375629e1a6b2d42c2d9205290b3d2fd4b1 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
@@ -24,21 +24,21 @@
#pragma once
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityController.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <VirtualRobot/VirtualRobot.h>
-
namespace armarx
{
TYPEDEF_PTRS_HANDLE(NJointCartesianVelocityController);
TYPEDEF_PTRS_HANDLE(NJointCartesianVelocityControllerControlData);
+
class NJointCartesianVelocityControllerControlData
{
public:
@@ -64,7 +64,6 @@ namespace armarx
float update(float dt, float error);
};
-
/**
* @brief The NJointCartesianVelocityController class
* @ingroup Library-RobotUnit-NJointControllers
@@ -75,39 +74,56 @@ namespace armarx
{
public:
using ConfigPtrT = NJointCartesianVelocityControllerConfigPtr;
- NJointCartesianVelocityController(RobotUnit* prov, const NJointCartesianVelocityControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+ NJointCartesianVelocityController(RobotUnit* prov,
+ const NJointCartesianVelocityControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&);
// NJointControllerInterface interface
std::string getClassName(const Ice::Current&) const override;
- WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current&) const override;
- void callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&) override;
+ WidgetDescription::StringWidgetDictionary
+ getFunctionDescriptions(const Ice::Current&) const override;
+ void callDescribedFunction(const std::string& name,
+ const StringVariantBaseMap& valueMap,
+ const Ice::Current&) override;
// NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
- static WidgetDescription::WidgetPtr GenerateConfigDescription(
- const VirtualRobot::RobotPtr&,
- const std::map<std::string, ConstControlDevicePtr>&,
- const std::map<std::string, ConstSensorDevicePtr>&);
-
- static NJointCartesianVelocityControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
- NJointCartesianVelocityController(
- RobotUnitPtr prov,
- NJointCartesianVelocityControllerConfigPtr config,
- const VirtualRobot::RobotPtr& r);
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
+ static WidgetDescription::WidgetPtr
+ GenerateConfigDescription(const VirtualRobot::RobotPtr&,
+ const std::map<std::string, ConstControlDevicePtr>&,
+ const std::map<std::string, ConstSensorDevicePtr>&);
+
+ static NJointCartesianVelocityControllerConfigPtr
+ GenerateConfigFromVariants(const StringVariantBaseMap& values);
+ NJointCartesianVelocityController(RobotUnitPtr prov,
+ NJointCartesianVelocityControllerConfigPtr config,
+ const VirtualRobot::RobotPtr& r);
// for TCPControlUnit
- void setVelocities(float xVel, float yVel, float zVel, float rollVel, float pitchVel, float yawVel, VirtualRobot::IKSolver::CartesianSelection mode);
+ void setVelocities(float xVel,
+ float yVel,
+ float zVel,
+ float rollVel,
+ float pitchVel,
+ float yawVel,
+ VirtualRobot::IKSolver::CartesianSelection mode);
void setAvoidJointLimitsKp(float kp);
std::string getNodeSetName() const;
static ::armarx::WidgetDescription::WidgetSeq createSliders();
- WidgetDescription::HBoxLayoutPtr createJointSlidersLayout(float min, float max, float defaultValue) const;
+ WidgetDescription::HBoxLayoutPtr
+ createJointSlidersLayout(float min, float max, float defaultValue) const;
static VirtualRobot::IKSolver::CartesianSelection ModeFromString(const std::string mode);
- static NJointCartesianVelocityControllerMode::CartesianSelection IceModeFromString(const std::string mode);
- static VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointCartesianVelocityControllerMode::CartesianSelection mode);
+ static NJointCartesianVelocityControllerMode::CartesianSelection
+ IceModeFromString(const std::string mode);
+ static VirtualRobot::IKSolver::CartesianSelection
+ ModeFromIce(const NJointCartesianVelocityControllerMode::CartesianSelection mode);
+
protected:
void rtPreActivateController() override;
void rtPostDeactivateController() override;
+
private:
std::vector<ControlTarget1DoFActuatorVelocity*> targets;
//std::vector<const SensorValue1DoFActuatorPosition*> sensors;
@@ -122,10 +138,18 @@ namespace armarx
// NJointCartesianVelocityControllerInterface interface
public:
- void setControllerTarget(float x, float y, float z, float roll, float pitch, float yaw, float avoidJointLimitsKp, NJointCartesianVelocityControllerMode::CartesianSelection mode, const Ice::Current&) override;
+ void setControllerTarget(float x,
+ float y,
+ float z,
+ float roll,
+ float pitch,
+ float yaw,
+ float avoidJointLimitsKp,
+ NJointCartesianVelocityControllerMode::CartesianSelection mode,
+ const Ice::Current&) override;
void setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&) override;
- void setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&) override;
+ void setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities,
+ const Ice::Current&) override;
};
} // namespace armarx
-
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
index 59f4b58be6c94973b3be1bf8de30e67a6d8f25b6..90ef15b6185b7424ef70714af3eec4b16b6a0ba6 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
@@ -23,20 +23,23 @@
*/
#include "NJointCartesianVelocityControllerWithRamp.h"
-#include "../RobotUnit.h"
+#include <VirtualRobot/RobotNodeSet.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-#include <VirtualRobot/RobotNodeSet.h>
+#include "../RobotUnit.h"
#define DEFAULT_TCP_STRING "default TCP"
namespace armarx
{
- NJointControllerRegistration<NJointCartesianVelocityControllerWithRamp> registrationControllerNJointCartesianVelocityControllerWithRamp("NJointCartesianVelocityControllerWithRamp");
+ NJointControllerRegistration<NJointCartesianVelocityControllerWithRamp>
+ registrationControllerNJointCartesianVelocityControllerWithRamp(
+ "NJointCartesianVelocityControllerWithRamp");
- std::string NJointCartesianVelocityControllerWithRamp::getClassName(const Ice::Current&) const
+ std::string
+ NJointCartesianVelocityControllerWithRamp::getClassName(const Ice::Current&) const
{
return "NJointCartesianVelocityControllerWithRamp";
}
@@ -58,8 +61,10 @@ namespace armarx
const SensorValueBase* sv = useSensorValue(jointName);
targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
- const SensorValue1DoFActuatorFilteredVelocity* filteredVelocitySensor = sv->asA<SensorValue1DoFActuatorFilteredVelocity>();
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorFilteredVelocity* filteredVelocitySensor =
+ sv->asA<SensorValue1DoFActuatorFilteredVelocity>();
+ const SensorValue1DoFActuatorVelocity* velocitySensor =
+ sv->asA<SensorValue1DoFActuatorVelocity>();
ARMARX_CHECK_EXPRESSION(filteredVelocitySensor || velocitySensor);
if (filteredVelocitySensor)
{
@@ -71,10 +76,12 @@ namespace armarx
ARMARX_IMPORTANT << "Using raw velocity for joint " << jointName;
velocitySensors.push_back(&velocitySensor->velocity);
}
-
};
- VirtualRobot::RobotNodePtr tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING) ? rns->getTCP() : rtGetRobot()->getRobotNode(config->tcpName);
+ VirtualRobot::RobotNodePtr tcp =
+ (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING)
+ ? rns->getTCP()
+ : rtGetRobot()->getRobotNode(config->tcpName);
ARMARX_CHECK_EXPRESSION(tcp) << config->tcpName;
nodeSetName = config->nodeSetName;
@@ -82,12 +89,17 @@ namespace armarx
KpJointLimitAvoidance = config->KpJointLimitAvoidance;
mode = ModeFromIce(config->mode);
- controller.reset(new CartesianVelocityControllerWithRamp(rns, Eigen::VectorXf::Zero(rns->getSize()), mode,
- config->maxPositionAcceleration, config->maxOrientationAcceleration, config->maxNullspaceAcceleration));
+ controller.reset(
+ new CartesianVelocityControllerWithRamp(rns,
+ Eigen::VectorXf::Zero(rns->getSize()),
+ mode,
+ config->maxPositionAcceleration,
+ config->maxOrientationAcceleration,
+ config->maxNullspaceAcceleration));
}
-
- void NJointCartesianVelocityControllerWithRamp::rtPreActivateController()
+ void
+ NJointCartesianVelocityControllerWithRamp::rtPreActivateController()
{
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
@@ -101,13 +113,17 @@ namespace armarx
ARMARX_IMPORTANT << "initial joint velocities: " << currentJointVelocities.transpose();
}
- void NJointCartesianVelocityControllerWithRamp::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ NJointCartesianVelocityControllerWithRamp::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
Eigen::VectorXf x;
if (mode == VirtualRobot::IKSolver::All)
{
x.resize(6);
- x << rtGetControlStruct().xVel, rtGetControlStruct().yVel, rtGetControlStruct().zVel, rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel, rtGetControlStruct().yawVel;
+ x << rtGetControlStruct().xVel, rtGetControlStruct().yVel, rtGetControlStruct().zVel,
+ rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel,
+ rtGetControlStruct().yawVel;
}
else if (mode == VirtualRobot::IKSolver::Position)
{
@@ -117,7 +133,8 @@ namespace armarx
else if (mode == VirtualRobot::IKSolver::Orientation)
{
x.resize(3);
- x << rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel, rtGetControlStruct().yawVel;
+ x << rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel,
+ rtGetControlStruct().yawVel;
}
else
{
@@ -125,25 +142,37 @@ namespace armarx
return;
}
- Eigen::VectorXf jnv = KpJointLimitAvoidance * controller->controller.calculateJointLimitAvoidance();
+ Eigen::VectorXf jnv =
+ KpJointLimitAvoidance * controller->controller.calculateJointLimitAvoidance();
jnv += controller->controller.calculateNullspaceVelocity(x, jointLimitAvoidanceScale, mode);
- Eigen::VectorXf jointTargetVelocities = controller->calculate(x, jnv, timeSinceLastIteration.toSecondsDouble());
+ Eigen::VectorXf jointTargetVelocities =
+ controller->calculate(x, jnv, timeSinceLastIteration.toSecondsDouble());
for (size_t i = 0; i < targets.size(); ++i)
{
targets.at(i)->velocity = jointTargetVelocities(i);
}
}
-
-
- void NJointCartesianVelocityControllerWithRamp::setMaxAccelerations(float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration, const Ice::Current&)
+ void
+ NJointCartesianVelocityControllerWithRamp::setMaxAccelerations(float maxPositionAcceleration,
+ float maxOrientationAcceleration,
+ float maxNullspaceAcceleration,
+ const Ice::Current&)
{
- controller->setMaxAccelerations(maxPositionAcceleration, maxOrientationAcceleration, maxNullspaceAcceleration);
+ controller->setMaxAccelerations(
+ maxPositionAcceleration, maxOrientationAcceleration, maxNullspaceAcceleration);
}
- void NJointCartesianVelocityControllerWithRamp::setTargetVelocity(float vx, float vy, float vz, float vrx, float vry, float vrz, const Ice::Current&)
+ void
+ NJointCartesianVelocityControllerWithRamp::setTargetVelocity(float vx,
+ float vy,
+ float vz,
+ float vrx,
+ float vry,
+ float vrz,
+ const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().xVel = vx;
getWriterControlStruct().yVel = vy;
getWriterControlStruct().zVel = vz;
@@ -153,19 +182,25 @@ namespace armarx
writeControlStruct();
}
- void NJointCartesianVelocityControllerWithRamp::setJointLimitAvoidanceScale(float jointLimitAvoidanceScale, const Ice::Current&)
+ void
+ NJointCartesianVelocityControllerWithRamp::setJointLimitAvoidanceScale(
+ float jointLimitAvoidanceScale,
+ const Ice::Current&)
{
this->jointLimitAvoidanceScale = jointLimitAvoidanceScale;
}
- void NJointCartesianVelocityControllerWithRamp::setKpJointLimitAvoidance(float KpJointLimitAvoidance, const Ice::Current&)
+ void
+ NJointCartesianVelocityControllerWithRamp::setKpJointLimitAvoidance(float KpJointLimitAvoidance,
+ const Ice::Current&)
{
this->KpJointLimitAvoidance = KpJointLimitAvoidance;
}
- void NJointCartesianVelocityControllerWithRamp::immediateHardStop(const Ice::Current&)
+ void
+ NJointCartesianVelocityControllerWithRamp::immediateHardStop(const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
controller->setCurrentJointVelocity(Eigen::VectorXf::Zero(velocitySensors.size()));
@@ -179,32 +214,33 @@ namespace armarx
writeControlStruct();
}
-
-
- void NJointCartesianVelocityControllerWithRamp::rtPostDeactivateController()
+ void
+ NJointCartesianVelocityControllerWithRamp::rtPostDeactivateController()
{
-
}
- const std::string& NJointCartesianVelocityControllerWithRamp::getNodeSetName() const
+ const std::string&
+ NJointCartesianVelocityControllerWithRamp::getNodeSetName() const
{
return nodeSetName;
}
- WidgetDescription::StringWidgetDictionary NJointCartesianVelocityControllerWithRamp::getFunctionDescriptions(const Ice::Current&) const
+ WidgetDescription::StringWidgetDictionary
+ NJointCartesianVelocityControllerWithRamp::getFunctionDescriptions(const Ice::Current&) const
{
- return
- {
- {"ControllerTarget", createTargetLayout()},
- {"ControllerParameters", createParameterLayout()}
- };
+ return {{"ControllerTarget", createTargetLayout()},
+ {"ControllerParameters", createParameterLayout()}};
}
- void NJointCartesianVelocityControllerWithRamp::callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&)
+ void
+ NJointCartesianVelocityControllerWithRamp::callDescribedFunction(
+ const std::string& name,
+ const StringVariantBaseMap& valueMap,
+ const Ice::Current&)
{
if (name == "ControllerTarget")
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().xVel = valueMap.at("x")->getFloat();
getWriterControlStruct().yVel = valueMap.at("y")->getFloat();
getWriterControlStruct().zVel = valueMap.at("z")->getFloat();
@@ -217,7 +253,7 @@ namespace armarx
}
else if (name == "ControllerParameters")
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
setMaxAccelerations(valueMap.at("maxPositionAcceleration")->getFloat(),
valueMap.at("maxOrientationAcceleration")->getFloat(),
valueMap.at("maxNullspaceAcceleration")->getFloat());
@@ -228,7 +264,8 @@ namespace armarx
}
}
- WidgetDescription::VBoxLayoutPtr NJointCartesianVelocityControllerWithRamp::createTargetLayout() const
+ WidgetDescription::VBoxLayoutPtr
+ NJointCartesianVelocityControllerWithRamp::createTargetLayout() const
{
LayoutBuilder layout;
layout.addSlider("x", -100, 100, 0);
@@ -242,17 +279,25 @@ namespace armarx
layout.addSlider("jointLimitAvoidanceScale", -10, 10, jointLimitAvoidanceScale);
return layout.getLayout();
}
- WidgetDescription::VBoxLayoutPtr NJointCartesianVelocityControllerWithRamp::createParameterLayout() const
+
+ WidgetDescription::VBoxLayoutPtr
+ NJointCartesianVelocityControllerWithRamp::createParameterLayout() const
{
LayoutBuilder layout;
- layout.addSlider("maxPositionAcceleration", 0, 1000, controller->getMaxPositionAcceleration());
- layout.addSlider("maxOrientationAcceleration", 0, 100, controller->getMaxOrientationAcceleration());
- layout.addSlider("maxNullspaceAcceleration", 0, 100, controller->getMaxNullspaceAcceleration());
+ layout.addSlider(
+ "maxPositionAcceleration", 0, 1000, controller->getMaxPositionAcceleration());
+ layout.addSlider(
+ "maxOrientationAcceleration", 0, 100, controller->getMaxOrientationAcceleration());
+ layout.addSlider(
+ "maxNullspaceAcceleration", 0, 100, controller->getMaxNullspaceAcceleration());
return layout.getLayout();
}
-
- WidgetDescription::WidgetPtr NJointCartesianVelocityControllerWithRamp::GenerateConfigDescription(const VirtualRobot::RobotPtr& robot, const std::map<std::string, ConstControlDevicePtr>& controlDevices, const std::map<std::string, ConstSensorDevicePtr>&)
+ WidgetDescription::WidgetPtr
+ NJointCartesianVelocityControllerWithRamp::GenerateConfigDescription(
+ const VirtualRobot::RobotPtr& robot,
+ const std::map<std::string, ConstControlDevicePtr>& controlDevices,
+ const std::map<std::string, ConstSensorDevicePtr>&)
{
using namespace armarx::WidgetDescription;
HBoxLayoutPtr layout = new HBoxLayout;
@@ -290,7 +335,7 @@ namespace armarx
boxMode->defaultIndex = 2;
- auto addSlider = [&](const std::string & label, float max, float defaultValue)
+ auto addSlider = [&](const std::string& label, float max, float defaultValue)
{
layout->children.emplace_back(new Label(false, label));
FloatSliderPtr floatSlider = new FloatSlider;
@@ -309,18 +354,23 @@ namespace armarx
return layout;
}
- NJointCartesianVelocityControllerWithRampConfigPtr NJointCartesianVelocityControllerWithRamp::GenerateConfigFromVariants(const StringVariantBaseMap& values)
+ NJointCartesianVelocityControllerWithRampConfigPtr
+ NJointCartesianVelocityControllerWithRamp::GenerateConfigFromVariants(
+ const StringVariantBaseMap& values)
{
- return new NJointCartesianVelocityControllerWithRampConfig(values.at("nodeSetName")->getString(), values.at("tcpName")->getString(),
- IceModeFromString(values.at("mode")->getString()),
- values.at("maxPositionAcceleration")->getFloat(),
- values.at("maxOrientationAcceleration")->getFloat(),
- values.at("maxNullspaceAcceleration")->getFloat(),
- values.at("KpJointLimitAvoidance")->getFloat(),
- values.at("jointLimitAvoidanceScale")->getFloat());
+ return new NJointCartesianVelocityControllerWithRampConfig(
+ values.at("nodeSetName")->getString(),
+ values.at("tcpName")->getString(),
+ IceModeFromString(values.at("mode")->getString()),
+ values.at("maxPositionAcceleration")->getFloat(),
+ values.at("maxOrientationAcceleration")->getFloat(),
+ values.at("maxNullspaceAcceleration")->getFloat(),
+ values.at("KpJointLimitAvoidance")->getFloat(),
+ values.at("jointLimitAvoidanceScale")->getFloat());
}
- VirtualRobot::IKSolver::CartesianSelection NJointCartesianVelocityControllerWithRamp::ModeFromString(const std::string mode)
+ VirtualRobot::IKSolver::CartesianSelection
+ NJointCartesianVelocityControllerWithRamp::ModeFromString(const std::string mode)
{
//ARMARX_IMPORTANT_S << "the mode is " << mode;
if (mode == "Position")
@@ -339,7 +389,8 @@ namespace armarx
return (VirtualRobot::IKSolver::CartesianSelection)0;
}
- CartesianSelectionMode::CartesianSelection NJointCartesianVelocityControllerWithRamp::IceModeFromString(const std::string mode)
+ CartesianSelectionMode::CartesianSelection
+ NJointCartesianVelocityControllerWithRamp::IceModeFromString(const std::string mode)
{
if (mode == "Position")
{
@@ -357,7 +408,9 @@ namespace armarx
return (CartesianSelectionMode::CartesianSelection)0;
}
- VirtualRobot::IKSolver::CartesianSelection NJointCartesianVelocityControllerWithRamp::ModeFromIce(const CartesianSelectionMode::CartesianSelection mode)
+ VirtualRobot::IKSolver::CartesianSelection
+ NJointCartesianVelocityControllerWithRamp::ModeFromIce(
+ const CartesianSelectionMode::CartesianSelection mode)
{
if (mode == CartesianSelectionMode::CartesianSelection::ePosition)
{
@@ -376,11 +429,4 @@ namespace armarx
}
-
-
} // namespace armarx
-
-
-
-
-
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
index 37a362c9b5215db5367938b136fcb85e9f665622..40bba081c0c63eaa83edb2454c03dfb73ce2bfe3 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
@@ -24,20 +24,21 @@
#pragma once
-#include "NJointControllerWithTripleBuffer.h"
+#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Robot.h>
+
+#include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.h>
+#include <RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h>
+
#include "../ControlTargets/ControlTarget1DoFActuator.h"
#include "../SensorValues/SensorValue1DoFActuator.h"
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.h>
+#include "NJointControllerWithTripleBuffer.h"
namespace armarx
{
TYPEDEF_PTRS_HANDLE(NJointCartesianVelocityControllerWithRamp);
-
class NJointCartesianVelocityControllerWithRampControlData
{
public:
@@ -53,20 +54,23 @@ namespace armarx
{
WidgetDescription::VBoxLayoutPtr vlayout = new WidgetDescription::VBoxLayout;
WidgetDescription::HBoxLayoutPtr hlayout;
+
public:
LayoutBuilder()
{
newLine();
}
- void newLine()
+ void
+ newLine()
{
using namespace WidgetDescription;
hlayout = new HBoxLayout;
vlayout->children.emplace_back(hlayout);
}
- void addSlider(const std::string& label, float min, float max, float value)
+ void
+ addSlider(const std::string& label, float min, float max, float value)
{
using namespace WidgetDescription;
hlayout->children.emplace_back(new Label(false, label));
@@ -78,50 +82,59 @@ namespace armarx
hlayout->children.emplace_back(slider);
}
- WidgetDescription::VBoxLayoutPtr getLayout() const
+ WidgetDescription::VBoxLayoutPtr
+ getLayout() const
{
return vlayout;
}
-
};
-
-
/**
* @brief The NJointCartesianVelocityControllerWithRamp class
* @ingroup Library-RobotUnit-NJointControllers
*/
class NJointCartesianVelocityControllerWithRamp :
- public NJointControllerWithTripleBuffer<NJointCartesianVelocityControllerWithRampControlData>,
+ public NJointControllerWithTripleBuffer<
+ NJointCartesianVelocityControllerWithRampControlData>,
public NJointCartesianVelocityControllerWithRampInterface
{
public:
using ConfigPtrT = NJointCartesianVelocityControllerWithRampConfigPtr;
- NJointCartesianVelocityControllerWithRamp(RobotUnit* robotUnit, const NJointCartesianVelocityControllerWithRampConfigPtr& config, const VirtualRobot::RobotPtr&);
+ NJointCartesianVelocityControllerWithRamp(
+ RobotUnit* robotUnit,
+ const NJointCartesianVelocityControllerWithRampConfigPtr& config,
+ const VirtualRobot::RobotPtr&);
// NJointControllerInterface interface
std::string getClassName(const Ice::Current&) const override;
// NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
- static WidgetDescription::WidgetPtr GenerateConfigDescription(
- const VirtualRobot::RobotPtr&,
- const std::map<std::string, ConstControlDevicePtr>&,
- const std::map<std::string, ConstSensorDevicePtr>&);
- static NJointCartesianVelocityControllerWithRampConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
- WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current&) const override;
- void callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
+
+ static WidgetDescription::WidgetPtr
+ GenerateConfigDescription(const VirtualRobot::RobotPtr&,
+ const std::map<std::string, ConstControlDevicePtr>&,
+ const std::map<std::string, ConstSensorDevicePtr>&);
+ static NJointCartesianVelocityControllerWithRampConfigPtr
+ GenerateConfigFromVariants(const StringVariantBaseMap& values);
+ WidgetDescription::StringWidgetDictionary
+ getFunctionDescriptions(const Ice::Current&) const override;
+ void callDescribedFunction(const std::string& name,
+ const StringVariantBaseMap& valueMap,
+ const Ice::Current&) override;
WidgetDescription::VBoxLayoutPtr createTargetLayout() const;
WidgetDescription::VBoxLayoutPtr createParameterLayout() const;
static VirtualRobot::IKSolver::CartesianSelection ModeFromString(const std::string mode);
static CartesianSelectionMode::CartesianSelection IceModeFromString(const std::string mode);
- static VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const CartesianSelectionMode::CartesianSelection mode);
+ static VirtualRobot::IKSolver::CartesianSelection
+ ModeFromIce(const CartesianSelectionMode::CartesianSelection mode);
protected:
void rtPreActivateController() override;
void rtPostDeactivateController() override;
+
private:
std::vector<ControlTarget1DoFActuatorVelocity*> targets;
//std::vector<const SensorValue1DoFActuatorPosition*> sensors;
@@ -137,13 +150,22 @@ namespace armarx
// NJointCartesianVelocityControllerWithRampInterface interface
public:
- void setMaxAccelerations(float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration, const Ice::Current& = Ice::emptyCurrent) override;
- void setTargetVelocity(float vx, float vy, float vz, float vrx, float vry, float vrz, const Ice::Current&) override;
- void setJointLimitAvoidanceScale(float jointLimitAvoidanceScale, const Ice::Current&) override;
+ void setMaxAccelerations(float maxPositionAcceleration,
+ float maxOrientationAcceleration,
+ float maxNullspaceAcceleration,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setTargetVelocity(float vx,
+ float vy,
+ float vz,
+ float vrx,
+ float vry,
+ float vrz,
+ const Ice::Current&) override;
+ void setJointLimitAvoidanceScale(float jointLimitAvoidanceScale,
+ const Ice::Current&) override;
void setKpJointLimitAvoidance(float KpJointLimitAvoidance, const Ice::Current&) override;
void immediateHardStop(const Ice::Current&) override;
const std::string& getNodeSetName() const;
};
} // namespace armarx
-
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
index 2b53ca18ef577c4ea609134e9d30351ca85100bc..697b087cc7f0b53edc3196b499c0f76dd3dec312 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
@@ -1,21 +1,21 @@
+#include "NJointCartesianWaypointController.h"
+
+#include <iomanip>
+
#include <ArmarXCore/util/CPPUtility/trace.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
-
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
-#include "NJointCartesianWaypointController.h"
-
-#include <iomanip>
-
namespace armarx
{
- std::ostream& operator<<(std::ostream& out, const NJointCartesianWaypointControllerRuntimeConfig& cfg)
+ std::ostream&
+ operator<<(std::ostream& out, const NJointCartesianWaypointControllerRuntimeConfig& cfg)
{
out << "maxPositionAcceleration " << cfg.wpCfg.maxPositionAcceleration << '\n'
<< "maxOrientationAcceleration " << cfg.wpCfg.maxOrientationAcceleration << '\n'
@@ -38,10 +38,12 @@ namespace armarx
return out;
}
+ NJointControllerRegistration<NJointCartesianWaypointController>
+ registrationControllerNJointCartesianWaypointController(
+ "NJointCartesianWaypointController");
- NJointControllerRegistration<NJointCartesianWaypointController> registrationControllerNJointCartesianWaypointController("NJointCartesianWaypointController");
-
- std::string NJointCartesianWaypointController::getClassName(const Ice::Current&) const
+ std::string
+ NJointCartesianWaypointController::getClassName(const Ice::Current&) const
{
return "NJointCartesianWaypointController";
}
@@ -59,11 +61,9 @@ namespace armarx
ARMARX_CHECK_NOT_NULL(_rtRobot);
_rtRns = _rtRobot->getRobotNodeSet(config->rns);
- ARMARX_CHECK_NOT_NULL(_rtRns)
- << "No rns " << config->rns;
+ ARMARX_CHECK_NOT_NULL(_rtRns) << "No rns " << config->rns;
_rtTcp = _rtRns->getTCP();
- ARMARX_CHECK_NOT_NULL(_rtTcp)
- << "No tcp in rns " << config->rns;
+ ARMARX_CHECK_NOT_NULL(_rtTcp) << "No tcp in rns " << config->rns;
_rtRobotRoot = _rtRobot->getRootNode();
}
@@ -73,8 +73,7 @@ namespace armarx
if (!config->ftName.empty())
{
const auto svalFT = useSensorValue<SensorValueForceTorque>(config->ftName);
- ARMARX_CHECK_NOT_NULL(svalFT)
- << "No sensor value of name " << config->ftName;
+ ARMARX_CHECK_NOT_NULL(svalFT) << "No sensor value of name " << config->ftName;
_rtForceSensor = &(svalFT->force);
_rtTorqueSensor = &(svalFT->force);
@@ -83,42 +82,43 @@ namespace armarx
const auto reportFrameName = sdev->getReportingFrame();
ARMARX_CHECK_EXPRESSION(!reportFrameName.empty())
- << VAROUT(sdev->getReportingFrame());
+ << VAROUT(sdev->getReportingFrame());
_rtFT = _rtRobot->getRobotNode(reportFrameName);
ARMARX_CHECK_NOT_NULL(_rtFT)
- << "No sensor report frame '" << reportFrameName
- << "' in robot";
+ << "No sensor report frame '" << reportFrameName << "' in robot";
}
}
//ctrl
{
- _rtJointVelocityFeedbackBuffer = Eigen::VectorXf::Zero(static_cast<int>(_rtRns->getSize()));
+ _rtJointVelocityFeedbackBuffer =
+ Eigen::VectorXf::Zero(static_cast<int>(_rtRns->getSize()));
VirtualRobot::RobotNodePtr referenceFrame = nullptr;
if (!config->referenceFrameName.empty())
{
referenceFrame = _rtRobot->getRobotNode(config->referenceFrameName);
ARMARX_CHECK_NOT_NULL(referenceFrame)
- << "No node with name '" << config->referenceFrameName << "' for referenceFrame in robot";
+ << "No node with name '" << config->referenceFrameName
+ << "' for referenceFrame in robot";
}
_rtWpController = std::make_unique<CartesianWaypointController>(
- _rtRns,
- _rtJointVelocityFeedbackBuffer,
- config->runCfg.wpCfg.maxPositionAcceleration,
- config->runCfg.wpCfg.maxOrientationAcceleration,
- config->runCfg.wpCfg.maxNullspaceAcceleration,
- nullptr,
- referenceFrame
- );
+ _rtRns,
+ _rtJointVelocityFeedbackBuffer,
+ config->runCfg.wpCfg.maxPositionAcceleration,
+ config->runCfg.wpCfg.maxOrientationAcceleration,
+ config->runCfg.wpCfg.maxNullspaceAcceleration,
+ nullptr,
+ referenceFrame);
_rtWpController->setConfig({});
for (size_t i = 0; i < _rtRns->getSize(); ++i)
{
std::string jointName = _rtRns->getNode(i)->getName();
- auto ct = useControlTarget<ControlTarget1DoFActuatorVelocity>(jointName, ControlModes::Velocity1DoF);
+ auto ct = useControlTarget<ControlTarget1DoFActuatorVelocity>(
+ jointName, ControlModes::Velocity1DoF);
auto sv = useSensorValue<SensorValue1DoFActuatorVelocity>(jointName);
ARMARX_CHECK_NOT_NULL(ct);
ARMARX_CHECK_NOT_NULL(sv);
@@ -134,7 +134,8 @@ namespace armarx
}
}
- void NJointCartesianWaypointController::rtPreActivateController()
+ void
+ NJointCartesianWaypointController::rtPreActivateController()
{
_publishIsAtTarget = false;
_rtForceOffset = Eigen::Vector3f::Zero();
@@ -159,7 +160,9 @@ namespace armarx
}
}
- void NJointCartesianWaypointController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ NJointCartesianWaypointController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
auto& rt2nonrtBuf = _tripRt2NonRt.getWriteBuffer();
@@ -201,9 +204,7 @@ namespace armarx
if (_rtForceSensor)
{
const Eigen::Vector3f ftInRoot =
- _rtFT->getTransformationTo(_rtRobotRoot)
- .topLeftCorner<3, 3>()
- .transpose() *
+ _rtFT->getTransformationTo(_rtRobotRoot).topLeftCorner<3, 3>().transpose() *
(*_rtForceSensor);
rt2nonrtBuf.ft.force = ftInRoot;
rt2nonrtBuf.ft.torque = *_rtTorqueSensor;
@@ -269,7 +270,8 @@ namespace armarx
}
else
{
- const Eigen::VectorXf& goal = _rtWpController->calculate(timeSinceLastIteration.toSecondsDouble());
+ const Eigen::VectorXf& goal =
+ _rtWpController->calculate(timeSinceLastIteration.toSecondsDouble());
//write targets
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(goal.size()), _rtVelTargets.size());
for (std::size_t idx = 0; idx < _rtVelTargets.size(); ++idx)
@@ -281,16 +283,21 @@ namespace armarx
_tripRt2NonRt.commitWrite();
}
- void NJointCartesianWaypointController::rtPostDeactivateController()
+ void
+ NJointCartesianWaypointController::rtPostDeactivateController()
{
-
}
- bool NJointCartesianWaypointController::hasReachedTarget(const Ice::Current&)
+ bool
+ NJointCartesianWaypointController::hasReachedTarget(const Ice::Current&)
{
return _publishIsAtTarget;
}
- void NJointCartesianWaypointController::setConfig(const NJointCartesianWaypointControllerRuntimeConfig& cfg, const Ice::Current&)
+
+ void
+ NJointCartesianWaypointController::setConfig(
+ const NJointCartesianWaypointControllerRuntimeConfig& cfg,
+ const Ice::Current&)
{
std::lock_guard g{_tripBufWpCtrlMut};
auto& w = _tripBufWpCtrl.getWriteBuffer();
@@ -299,7 +306,10 @@ namespace armarx
_tripBufWpCtrl.commitWrite();
ARMARX_IMPORTANT << "set new config\n" << cfg;
}
- void NJointCartesianWaypointController::setWaypoints(const std::vector<Eigen::Matrix4f>& wps, const Ice::Current&)
+
+ void
+ NJointCartesianWaypointController::setWaypoints(const std::vector<Eigen::Matrix4f>& wps,
+ const Ice::Current&)
{
std::lock_guard g{_tripBufWpCtrlMut};
auto& w = _tripBufWpCtrl.getWriteBuffer();
@@ -308,7 +318,9 @@ namespace armarx
_tripBufWpCtrl.commitWrite();
ARMARX_IMPORTANT << "set new waypoints\n" << wps;
}
- void NJointCartesianWaypointController::setWaypoint(const Eigen::Matrix4f& wp, const Ice::Current&)
+
+ void
+ NJointCartesianWaypointController::setWaypoint(const Eigen::Matrix4f& wp, const Ice::Current&)
{
std::lock_guard g{_tripBufWpCtrlMut};
auto& w = _tripBufWpCtrl.getWriteBuffer();
@@ -318,7 +330,9 @@ namespace armarx
_tripBufWpCtrl.commitWrite();
ARMARX_IMPORTANT << "set new waypoint\n" << wp;
}
- void NJointCartesianWaypointController::setWaypointAx(const Ice::FloatSeq& data, const Ice::Current&)
+
+ void
+ NJointCartesianWaypointController::setWaypointAx(const Ice::FloatSeq& data, const Ice::Current&)
{
Eigen::Matrix4f wp(data.data());
std::lock_guard g{_tripBufWpCtrlMut};
@@ -330,10 +344,11 @@ namespace armarx
ARMARX_IMPORTANT << "set new waypoint\n" << wp;
}
-
- void NJointCartesianWaypointController::setConfigAndWaypoints(const NJointCartesianWaypointControllerRuntimeConfig& cfg,
- const std::vector<Eigen::Matrix4f>& wps,
- const Ice::Current&)
+ void
+ NJointCartesianWaypointController::setConfigAndWaypoints(
+ const NJointCartesianWaypointControllerRuntimeConfig& cfg,
+ const std::vector<Eigen::Matrix4f>& wps,
+ const Ice::Current&)
{
std::lock_guard g{_tripBufWpCtrlMut};
auto& w = _tripBufWpCtrl.getWriteBuffer();
@@ -344,9 +359,12 @@ namespace armarx
_tripBufWpCtrl.commitWrite();
ARMARX_IMPORTANT << "set new config\n" << cfg << " and new waypoints\n" << wps;
}
- void NJointCartesianWaypointController::setConfigAndWaypoint(const NJointCartesianWaypointControllerRuntimeConfig& cfg,
- const Eigen::Matrix4f& wp,
- const Ice::Current&)
+
+ void
+ NJointCartesianWaypointController::setConfigAndWaypoint(
+ const NJointCartesianWaypointControllerRuntimeConfig& cfg,
+ const Eigen::Matrix4f& wp,
+ const Ice::Current&)
{
std::lock_guard g{_tripBufWpCtrlMut};
auto& w = _tripBufWpCtrl.getWriteBuffer();
@@ -358,7 +376,9 @@ namespace armarx
_tripBufWpCtrl.commitWrite();
ARMARX_IMPORTANT << "set new config\n" << cfg << "and a new waypoint\n" << wp;
}
- void NJointCartesianWaypointController::setNullVelocity()
+
+ void
+ NJointCartesianWaypointController::setNullVelocity()
{
for (auto ptr : _rtVelTargets)
{
@@ -366,38 +386,47 @@ namespace armarx
}
}
- bool NJointCartesianWaypointController::hasReachedForceLimit(const Ice::Current&)
+ bool
+ NJointCartesianWaypointController::hasReachedForceLimit(const Ice::Current&)
{
ARMARX_CHECK_NOT_NULL(_rtForceSensor);
return _publishIsAtForceLimit;
}
- FTSensorValue NJointCartesianWaypointController::getFTSensorValue(const Ice::Current&)
+
+ FTSensorValue
+ NJointCartesianWaypointController::getFTSensorValue(const Ice::Current&)
{
ARMARX_CHECK_NOT_NULL(_rtForceSensor);
std::lock_guard g{_tripRt2NonRtMutex};
return _tripRt2NonRt.getUpToDateReadBuffer().ft;
}
- void NJointCartesianWaypointController::setCurrentFTAsOffset(const Ice::Current&)
+
+ void
+ NJointCartesianWaypointController::setCurrentFTAsOffset(const Ice::Current&)
{
ARMARX_CHECK_NOT_NULL(_rtForceSensor);
_setFTOffset = true;
}
- void NJointCartesianWaypointController::setVisualizationRobotGlobalPose(const Eigen::Matrix4f& p, const Ice::Current&)
+ void
+ NJointCartesianWaypointController::setVisualizationRobotGlobalPose(const Eigen::Matrix4f& p,
+ const Ice::Current&)
{
std::lock_guard g{_tripFakeRobotGPWriteMutex};
_tripFakeRobotGP.getWriteBuffer() = p;
_tripFakeRobotGP.commitWrite();
}
- void NJointCartesianWaypointController::resetVisualizationRobotGlobalPose(const Ice::Current&)
+ void
+ NJointCartesianWaypointController::resetVisualizationRobotGlobalPose(const Ice::Current&)
{
std::lock_guard g{_tripFakeRobotGPWriteMutex};
_tripFakeRobotGP.getWriteBuffer()(0, 0) = std::nanf("");
_tripFakeRobotGP.commitWrite();
}
- void NJointCartesianWaypointController::stopMovement(const Ice::Current&)
+ void
+ NJointCartesianWaypointController::stopMovement(const Ice::Current&)
{
std::lock_guard g{_tripBufWpCtrlMut};
auto& w = _tripBufWpCtrl.getWriteBuffer();
@@ -408,10 +437,10 @@ namespace armarx
ARMARX_IMPORTANT << "stop movement by setting 0 waypoints";
}
- void NJointCartesianWaypointController::onPublish(
- const SensorAndControl&,
- const DebugDrawerInterfacePrx& drawer,
- const DebugObserverInterfacePrx& obs)
+ void
+ NJointCartesianWaypointController::onPublish(const SensorAndControl&,
+ const DebugDrawerInterfacePrx& drawer,
+ const DebugObserverInterfacePrx& obs)
{
const std::size_t wpsNum = _publishWpsNum;
const std::size_t wpsCur = wpsNum ? _publishWpsCur + 1 : 0;
@@ -439,13 +468,10 @@ namespace armarx
obs->setDebugChannel(getInstanceName(), datafields);
}
- ARMARX_INFO << deactivateSpam(1) << std::setprecision(4)
- << "At waypoint " << wpsCur << " / " << wpsNum
- << " (last " << _publishIsAtTarget
- << ", ft limit " << _publishIsAtForceLimit
- << ", perror " << errorPos << " / " << errorPosMax
- << ", oerror " << errorOri << " / " << errorOriMax
- << ')';
+ ARMARX_INFO << deactivateSpam(1) << std::setprecision(4) << "At waypoint " << wpsCur
+ << " / " << wpsNum << " (last " << _publishIsAtTarget << ", ft limit "
+ << _publishIsAtForceLimit << ", perror " << errorPos << " / " << errorPosMax
+ << ", oerror " << errorOri << " / " << errorOriMax << ')';
{
std::lock_guard g{_tripRt2NonRtMutex};
@@ -460,11 +486,12 @@ namespace armarx
const Eigen::Matrix4f gtcptarg = gp * buf.tcpTarg;
drawer->setPoseVisu(getName(), "tcp", new Pose(gtcp));
drawer->setPoseVisu(getName(), "tcptarg", new Pose(gtcptarg));
- drawer->setLineVisu(
- getName(), "tcp2targ",
- new Vector3(Eigen::Vector3f{gtcp.topRightCorner<3, 1>()}),
- new Vector3(Eigen::Vector3f{gtcptarg.topRightCorner<3, 1>()}),
- 3, {0, 0, 1, 1});
+ drawer->setLineVisu(getName(),
+ "tcp2targ",
+ new Vector3(Eigen::Vector3f{gtcp.topRightCorner<3, 1>()}),
+ new Vector3(Eigen::Vector3f{gtcptarg.topRightCorner<3, 1>()}),
+ 3,
+ {0, 0, 1, 1});
}
else
{
@@ -476,20 +503,27 @@ namespace armarx
{
const Eigen::Matrix4f gft = gp * buf.ftInRoot;
const Vector3Ptr pos = new Vector3{Eigen::Vector3f{gft.topRightCorner<3, 1>()}};
- drawer->setArrowVisu(
- getName(), "force", pos,
- new Vector3{buf.ft.force.normalized()},
- {1, 1, 0, 1}, buf.ft.force.norm(), 2.5);
- drawer->setArrowVisu(
- getName(), "forceUsed", pos,
- new Vector3{buf.ftUsed.normalized()},
- {1, 0.5, 0, 1}, buf.ftUsed.norm(), 2.5);
+ drawer->setArrowVisu(getName(),
+ "force",
+ pos,
+ new Vector3{buf.ft.force.normalized()},
+ {1, 1, 0, 1},
+ buf.ft.force.norm(),
+ 2.5);
+ drawer->setArrowVisu(getName(),
+ "forceUsed",
+ pos,
+ new Vector3{buf.ftUsed.normalized()},
+ {1, 0.5, 0, 1},
+ buf.ftUsed.norm(),
+ 2.5);
}
}
}
- int NJointCartesianWaypointController::getCurrentWaypointIndex(const Ice::Current&)
+ int
+ NJointCartesianWaypointController::getCurrentWaypointIndex(const Ice::Current&)
{
return _publishWpsCur;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
index affada4d1467a0c313a1d32b708469a5c703ca50..dce6aed44f8bb931baf2a598c06f127cb9b20d66 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
@@ -1,13 +1,12 @@
#pragma once
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
-
-#include <RobotAPI/libraries/core/CartesianWaypointController.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
-#include <VirtualRobot/VirtualRobot.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.h>
+#include <RobotAPI/libraries/core/CartesianWaypointController.h>
namespace armarx
{
@@ -24,10 +23,9 @@ namespace armarx
{
public:
using ConfigPtrT = NJointCartesianWaypointControllerConfigPtr;
- NJointCartesianWaypointController(
- RobotUnit* robotUnit,
- const NJointCartesianWaypointControllerConfigPtr& config,
- const VirtualRobot::RobotPtr&);
+ NJointCartesianWaypointController(RobotUnit* robotUnit,
+ const NJointCartesianWaypointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&);
// NJointControllerInterface interface
std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override;
@@ -35,7 +33,8 @@ namespace armarx
// void callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current& = Ice::emptyCurrent) override;
// NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
// static WidgetDescription::WidgetPtr GenerateConfigDescription(
// const VirtualRobot::RobotPtr&,
// const std::map<std::string, ConstControlDevicePtr>&,
@@ -47,26 +46,38 @@ namespace armarx
// NJointCartesianWaypointControllerConfigPtr config,
// const VirtualRobot::RobotPtr& r);
- void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
+ void onPublish(const SensorAndControl&,
+ const DebugDrawerInterfacePrx&,
+ const DebugObserverInterfacePrx&) override;
public:
bool hasReachedTarget(const Ice::Current& = Ice::emptyCurrent) override;
bool hasReachedForceLimit(const Ice::Current& = Ice::emptyCurrent) override;
int getCurrentWaypointIndex(const Ice::Current& = Ice::emptyCurrent) override;
- void setConfig(const NJointCartesianWaypointControllerRuntimeConfig& cfg, const Ice::Current& = Ice::emptyCurrent) override;
- void setWaypoints(const std::vector<Eigen::Matrix4f>& wps, const Ice::Current& = Ice::emptyCurrent) override;
- void setWaypointAx(const Ice::FloatSeq& data, const Ice::Current& = Ice::emptyCurrent) override;
- void setWaypoint(const Eigen::Matrix4f& wp, const Ice::Current& = Ice::emptyCurrent) override;
- void setConfigAndWaypoints(const NJointCartesianWaypointControllerRuntimeConfig& cfg, const std::vector<Eigen::Matrix4f>& wps, const Ice::Current& = Ice::emptyCurrent) override;
- void setConfigAndWaypoint(const NJointCartesianWaypointControllerRuntimeConfig& cfg, const Eigen::Matrix4f& wp, const Ice::Current& = Ice::emptyCurrent) override;
+ void setConfig(const NJointCartesianWaypointControllerRuntimeConfig& cfg,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setWaypoints(const std::vector<Eigen::Matrix4f>& wps,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setWaypointAx(const Ice::FloatSeq& data,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setWaypoint(const Eigen::Matrix4f& wp,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setConfigAndWaypoints(const NJointCartesianWaypointControllerRuntimeConfig& cfg,
+ const std::vector<Eigen::Matrix4f>& wps,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setConfigAndWaypoint(const NJointCartesianWaypointControllerRuntimeConfig& cfg,
+ const Eigen::Matrix4f& wp,
+ const Ice::Current& = Ice::emptyCurrent) override;
void stopMovement(const Ice::Current& = Ice::emptyCurrent) override;
FTSensorValue getFTSensorValue(const Ice::Current& = Ice::emptyCurrent) override;
void setCurrentFTAsOffset(const Ice::Current& = Ice::emptyCurrent) override;
- void setVisualizationRobotGlobalPose(const Eigen::Matrix4f& p, const Ice::Current& = Ice::emptyCurrent) override;
+ void setVisualizationRobotGlobalPose(const Eigen::Matrix4f& p,
+ const Ice::Current& = Ice::emptyCurrent) override;
void resetVisualizationRobotGlobalPose(const Ice::Current& = Ice::emptyCurrent) override;
+
protected:
void rtPreActivateController() override;
void rtPostDeactivateController() override;
@@ -100,53 +111,53 @@ namespace armarx
using Ctrl = CartesianWaypointController;
//rt data
- VirtualRobot::RobotPtr _rtRobot;
- VirtualRobot::RobotNodeSetPtr _rtRns;
- VirtualRobot::RobotNodePtr _rtTcp;
- VirtualRobot::RobotNodePtr _rtFT;
- VirtualRobot::RobotNodePtr _rtRobotRoot;
+ VirtualRobot::RobotPtr _rtRobot;
+ VirtualRobot::RobotNodeSetPtr _rtRns;
+ VirtualRobot::RobotNodePtr _rtTcp;
+ VirtualRobot::RobotNodePtr _rtFT;
+ VirtualRobot::RobotNodePtr _rtRobotRoot;
- std::unique_ptr<Ctrl> _rtWpController;
- Eigen::VectorXf _rtJointVelocityFeedbackBuffer;
+ std::unique_ptr<Ctrl> _rtWpController;
+ Eigen::VectorXf _rtJointVelocityFeedbackBuffer;
- std::vector<const float*> _rtVelSensors;
- std::vector<float*> _rtVelTargets;
+ std::vector<const float*> _rtVelSensors;
+ std::vector<float*> _rtVelTargets;
- const Eigen::Vector3f* _rtForceSensor = nullptr;
- const Eigen::Vector3f* _rtTorqueSensor = nullptr;
+ const Eigen::Vector3f* _rtForceSensor = nullptr;
+ const Eigen::Vector3f* _rtTorqueSensor = nullptr;
- Eigen::Vector3f _rtForceOffset;
+ Eigen::Vector3f _rtForceOffset;
- float _rtForceThreshold = -1;
- bool _rtOptimizeNullspaceIfTargetWasReached = false;
- bool _rtForceThresholdInRobotRootZ = true;
- bool _rtHasWps = false;
- bool _rtStopConditionReached = false;
+ float _rtForceThreshold = -1;
+ bool _rtOptimizeNullspaceIfTargetWasReached = false;
+ bool _rtForceThresholdInRobotRootZ = true;
+ bool _rtHasWps = false;
+ bool _rtStopConditionReached = false;
//flags
- std::atomic_bool _publishIsAtTarget{false};
- std::atomic_bool _publishIsAtForceLimit{false};
- std::atomic_bool _setFTOffset{false};
+ std::atomic_bool _publishIsAtTarget{false};
+ std::atomic_bool _publishIsAtForceLimit{false};
+ std::atomic_bool _setFTOffset{false};
//buffers
- mutable std::recursive_mutex _tripBufWpCtrlMut;
- WriteBufferedTripleBuffer<CtrlData> _tripBufWpCtrl;
+ mutable std::recursive_mutex _tripBufWpCtrlMut;
+ WriteBufferedTripleBuffer<CtrlData> _tripBufWpCtrl;
- mutable std::recursive_mutex _tripRt2NonRtMutex;
- TripleBuffer<RtToNonRtData> _tripRt2NonRt;
+ mutable std::recursive_mutex _tripRt2NonRtMutex;
+ TripleBuffer<RtToNonRtData> _tripRt2NonRt;
- mutable std::recursive_mutex _tripFakeRobotGPWriteMutex;
- TripleBuffer<Eigen::Matrix4f> _tripFakeRobotGP;
+ mutable std::recursive_mutex _tripFakeRobotGPWriteMutex;
+ TripleBuffer<Eigen::Matrix4f> _tripFakeRobotGP;
//publish data
- std::atomic_size_t _publishWpsNum{0};
- std::atomic_size_t _publishWpsCur{0};
- std::atomic<float> _publishErrorPos{0};
- std::atomic<float> _publishErrorOri{0};
- std::atomic<float> _publishErrorPosMax{0};
- std::atomic<float> _publishErrorOriMax{0};
- std::atomic<float> _publishForceThreshold{0};
- std::atomic<float> _publishForceCurrent{0};
- std::atomic_bool _publishForceThresholdInRobotRootZ{0};
+ std::atomic_size_t _publishWpsNum{0};
+ std::atomic_size_t _publishWpsCur{0};
+ std::atomic<float> _publishErrorPos{0};
+ std::atomic<float> _publishErrorOri{0};
+ std::atomic<float> _publishErrorPosMax{0};
+ std::atomic<float> _publishErrorOriMax{0};
+ std::atomic<float> _publishForceThreshold{0};
+ std::atomic<float> _publishForceCurrent{0};
+ std::atomic_bool _publishForceThresholdInRobotRootZ{0};
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
index a127273c7551352855d23e762cb70b5ff3b5bfba..136df3ceb422d637cad81288ee3082003c322135 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
@@ -20,23 +20,23 @@
* GNU General Public License
*/
-#include "NJointControllerBase.h"
-#include "NJointControllerRegistry.h"
+#include <atomic>
-#include <ArmarXCore/core/util/algorithm.h>
#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/util/algorithm.h>
#include "../RobotUnit.h"
-
-#include <atomic>
+#include "NJointControllerBase.h"
+#include "NJointControllerRegistry.h"
namespace armarx
{
- RobotUnit* getRobotUnit(RobotUnitModule::ControllerManagement* cmngr)
+ RobotUnit*
+ getRobotUnit(RobotUnitModule::ControllerManagement* cmngr)
{
return cmngr->_modulePtr<RobotUnit>();
}
-}
+} // namespace armarx
namespace armarx::RobotUnitModule
{
@@ -48,7 +48,11 @@ namespace armarx::RobotUnitModule
class DevicesAttorneyForNJointController
{
friend class ::armarx::NJointControllerBase;
- static JointController* GetJointController(Devices& d, const std::string& deviceName, const std::string& controlMode)
+
+ static JointController*
+ GetJointController(Devices& d,
+ const std::string& deviceName,
+ const std::string& controlMode)
{
if (d.controlDevices.has(deviceName))
{
@@ -63,20 +67,16 @@ namespace armarx::RobotUnitModule
return nullptr;
}
};
-}
-
-
+} // namespace armarx::RobotUnitModule
thread_local bool armarx::NJointControllerRegistryEntry::ConstructorIsRunning_ = false;
namespace armarx
{
- const NJointControllerBasePtr NJointControllerBase::NullPtr
- {
- nullptr
- };
+ const NJointControllerBasePtr NJointControllerBase::NullPtr{nullptr};
- NJointControllerDescription NJointControllerBase::getControllerDescription(const Ice::Current&) const
+ NJointControllerDescription
+ NJointControllerBase::getControllerDescription(const Ice::Current&) const
{
ARMARX_TRACE;
NJointControllerDescription d;
@@ -90,7 +90,8 @@ namespace armarx
return d;
}
- std::optional<std::vector<char> > NJointControllerBase::isNotInConflictWith(const std::vector<char>& used) const
+ std::optional<std::vector<char>>
+ NJointControllerBase::isNotInConflictWith(const std::vector<char>& used) const
{
ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(used.size() == controlDeviceUsedBitmap.size());
@@ -105,12 +106,12 @@ namespace armarx
}
result.at(i) = true;
}
-
}
return result;
}
- NJointControllerStatus NJointControllerBase::getControllerStatus(const Ice::Current&) const
+ NJointControllerStatus
+ NJointControllerBase::getControllerStatus(const Ice::Current&) const
{
ARMARX_TRACE;
NJointControllerStatus s;
@@ -118,11 +119,14 @@ namespace armarx
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();
+ s.timestampUSec = std::chrono::duration_cast<std::chrono::microseconds>(
+ std::chrono::high_resolution_clock::now().time_since_epoch())
+ .count();
return s;
}
- NJointControllerDescriptionWithStatus NJointControllerBase::getControllerDescriptionWithStatus(const Ice::Current&) const
+ NJointControllerDescriptionWithStatus
+ NJointControllerBase::getControllerDescriptionWithStatus(const Ice::Current&) const
{
NJointControllerDescriptionWithStatus ds;
ds.description = getControllerDescription();
@@ -130,31 +134,39 @@ namespace armarx
return ds;
}
-// RobotUnitInterfacePrx NJointControllerBase::getRobotUnit(const Ice::Current&) const
-// {
-// return RobotUnitInterfacePrx::uncheckedCast(robotUnit.getProxy(-1));
-// }
+ // RobotUnitInterfacePrx NJointControllerBase::getRobotUnit(const Ice::Current&) const
+ // {
+ // return RobotUnitInterfacePrx::uncheckedCast(robotUnit.getProxy(-1));
+ // }
- void NJointControllerBase::activateController(const Ice::Current&)
+ void
+ NJointControllerBase::activateController(const Ice::Current&)
{
robotUnit.activateNJointController(this);
}
- void NJointControllerBase::deactivateController(const Ice::Current&)
+ void
+ NJointControllerBase::deactivateController(const Ice::Current&)
{
robotUnit.deactivateNJointController(this);
}
- void NJointControllerBase::deleteController(const Ice::Current&)
+ void
+ NJointControllerBase::deleteController(const Ice::Current&)
{
robotUnit.deleteNJointController(this);
}
- void NJointControllerBase::deactivateAndDeleteController(const Ice::Current&)
+
+ void
+ NJointControllerBase::deactivateAndDeleteController(const Ice::Current&)
{
robotUnit.deactivateAndDeleteNJointController(this);
}
- void NJointControllerBase::publish(const SensorAndControl& sac, const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer)
+ void
+ NJointControllerBase::publish(const SensorAndControl& sac,
+ const DebugDrawerInterfacePrx& draw,
+ const DebugObserverInterfacePrx& observer)
{
ARMARX_TRACE;
const bool active = isActive;
@@ -169,11 +181,14 @@ namespace armarx
}
catch (std::exception& e)
{
- ARMARX_WARNING << "onPublishActivation of '" << getInstanceName() << "' threw an exception!\nWhat:\n" << e.what();
+ ARMARX_WARNING << "onPublishActivation of '" << getInstanceName()
+ << "' threw an exception!\nWhat:\n"
+ << e.what();
}
catch (...)
{
- ARMARX_WARNING << "onPublishActivation of '" << getInstanceName() << "' threw an exception!";
+ ARMARX_WARNING << "onPublishActivation of '" << getInstanceName()
+ << "' threw an exception!";
}
}
else
@@ -185,11 +200,14 @@ namespace armarx
}
catch (std::exception& e)
{
- ARMARX_WARNING << "onPublishDeactivation of '" << getInstanceName() << "' threw an exception!\nWhat:\n" << e.what();
+ ARMARX_WARNING << "onPublishDeactivation of '" << getInstanceName()
+ << "' threw an exception!\nWhat:\n"
+ << e.what();
}
catch (...)
{
- ARMARX_WARNING << "onPublishDeactivation of '" << getInstanceName() << "' threw an exception!";
+ ARMARX_WARNING << "onPublishDeactivation of '" << getInstanceName()
+ << "' threw an exception!";
}
}
}
@@ -201,7 +219,9 @@ namespace armarx
}
catch (std::exception& e)
{
- ARMARX_WARNING << "onPublish of '" << getInstanceName() << "' threw an exception!\nWhat:\n" << e.what();
+ ARMARX_WARNING << "onPublish of '" << getInstanceName()
+ << "' threw an exception!\nWhat:\n"
+ << e.what();
}
catch (...)
{
@@ -210,7 +230,9 @@ namespace armarx
}
}
- void NJointControllerBase::deactivatePublish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer)
+ void
+ NJointControllerBase::deactivatePublish(const DebugDrawerInterfacePrx& draw,
+ const DebugObserverInterfacePrx& observer)
{
ARMARX_TRACE;
if (publishActive.exchange(false))
@@ -220,7 +242,8 @@ namespace armarx
}
}
- void NJointControllerBase::rtActivateController()
+ void
+ NJointControllerBase::rtActivateController()
{
if (!isActive)
{
@@ -231,7 +254,8 @@ namespace armarx
}
}
- void NJointControllerBase::rtDeactivateController()
+ void
+ NJointControllerBase::rtDeactivateController()
{
if (isActive)
{
@@ -240,58 +264,66 @@ namespace armarx
}
}
- void NJointControllerBase::rtDeactivateControllerBecauseOfError()
+ void
+ NJointControllerBase::rtDeactivateControllerBecauseOfError()
{
deactivatedBecauseOfError = true;
rtDeactivateController();
}
- armarx::ConstSensorDevicePtr armarx::NJointControllerBase::peekSensorDevice(const std::string& deviceName) const
+ armarx::ConstSensorDevicePtr
+ armarx::NJointControllerBase::peekSensorDevice(const std::string& deviceName) const
{
- ARMARX_CHECK_EXPRESSION(
- NJointControllerRegistryEntry::ConstructorIsRunning()) <<
- "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController.";
+ ARMARX_CHECK_EXPRESSION(NJointControllerRegistryEntry::ConstructorIsRunning())
+ << "The function '" << BOOST_CURRENT_FUNCTION
+ << "' must only be called during the construction of an NJointController.";
return RobotUnitModule::Devices::Instance().getSensorDevice(deviceName);
}
- ConstControlDevicePtr NJointControllerBase::peekControlDevice(const std::string& deviceName) const
+ ConstControlDevicePtr
+ NJointControllerBase::peekControlDevice(const std::string& deviceName) const
{
- ARMARX_CHECK_EXPRESSION(
- NJointControllerRegistryEntry::ConstructorIsRunning()) <<
- "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController.";
+ ARMARX_CHECK_EXPRESSION(NJointControllerRegistryEntry::ConstructorIsRunning())
+ << "The function '" << BOOST_CURRENT_FUNCTION
+ << "' must only be called during the construction of an NJointController.";
return RobotUnitModule::Devices::Instance().getControlDevice(deviceName);
}
- const VirtualRobot::RobotPtr& NJointControllerBase::useSynchronizedRtRobot(bool updateCollisionModel)
+ const VirtualRobot::RobotPtr&
+ NJointControllerBase::useSynchronizedRtRobot(bool updateCollisionModel)
{
- ARMARX_CHECK_EXPRESSION(
- NJointControllerRegistryEntry::ConstructorIsRunning()) <<
- "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController.";
+ ARMARX_CHECK_EXPRESSION(NJointControllerRegistryEntry::ConstructorIsRunning())
+ << "The function '" << BOOST_CURRENT_FUNCTION
+ << "' must only be called during the construction of an NJointController.";
ARMARX_CHECK_IS_NULL(rtRobot) << "useSynchronizedRtRobot was already called";
rtRobot = RobotUnitModule::RobotData::Instance().cloneRobot(updateCollisionModel);
rtRobotNodes = rtRobot->getRobotNodes();
return rtRobot;
}
- void NJointControllerBase::onInitComponent()
+ void
+ NJointControllerBase::onInitComponent()
{
ARMARX_TRACE;
onInitNJointController();
}
- void NJointControllerBase::onConnectComponent()
+ void
+ NJointControllerBase::onConnectComponent()
{
ARMARX_TRACE;
onConnectNJointController();
}
- void NJointControllerBase::onDisconnectComponent()
+ void
+ NJointControllerBase::onDisconnectComponent()
{
ARMARX_TRACE;
onDisconnectNJointController();
}
- void NJointControllerBase::onExitComponent()
+ void
+ NJointControllerBase::onExitComponent()
{
ARMARX_TRACE;
onExitNJointController();
@@ -322,14 +354,14 @@ namespace armarx
{
ARMARX_INFO << "Still waiting for " << name << " thread handle!";
}
- else if (status == std::future_status::ready || status == std::future_status::deferred)
+ else if (status == std::future_status::ready ||
+ status == std::future_status::deferred)
{
ARMARX_VERBOSE << "Joining " << name << " task";
handle->join();
handle.reset();
}
- }
- while (status != std::future_status::ready);
+ } while (status != std::future_status::ready);
}
catch (...)
{
@@ -340,19 +372,20 @@ namespace armarx
threadHandles.clear();
}
- ThreadPoolPtr NJointControllerBase::getThreadPool() const
+ ThreadPoolPtr
+ NJointControllerBase::getThreadPool() const
{
ARMARX_CHECK_EXPRESSION(Application::getInstance());
return Application::getInstance()->getThreadPool();
}
-
- const SensorValueBase* NJointControllerBase::useSensorValue(const std::string& deviceName) const
+ const SensorValueBase*
+ NJointControllerBase::useSensorValue(const std::string& deviceName) const
{
ARMARX_TRACE;
- ARMARX_CHECK_EXPRESSION(
- NJointControllerRegistryEntry::ConstructorIsRunning()) <<
- "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController.";
+ ARMARX_CHECK_EXPRESSION(NJointControllerRegistryEntry::ConstructorIsRunning())
+ << "The function '" << BOOST_CURRENT_FUNCTION
+ << "' must only be called during the construction of an NJointController.";
auto dev = peekSensorDevice(deviceName);
if (!dev)
{
@@ -363,7 +396,7 @@ namespace armarx
}
NJointControllerBase::NJointControllerBase() :
- robotUnit(RobotUnitModule::ModuleBase::Instance< ::armarx::RobotUnit>()),
+ robotUnit(RobotUnitModule::ModuleBase::Instance<::armarx::RobotUnit>()),
controlDeviceUsedBitmap(robotUnit.getNumberOfControlDevices(), 0)
{
controlDeviceUsedIndices.reserve(robotUnit.getNumberOfControlDevices());
@@ -373,21 +406,27 @@ namespace armarx
{
}
- ControlTargetBase* NJointControllerBase::useControlTarget(const std::string& deviceName, const std::string& controlMode)
+ ControlTargetBase*
+ NJointControllerBase::useControlTarget(const std::string& deviceName,
+ const std::string& controlMode)
{
ARMARX_TRACE;
- ARMARX_CHECK_EXPRESSION(
- NJointControllerRegistryEntry::ConstructorIsRunning()) <<
- "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController.";
- ARMARX_CHECK_EQUAL(
- controlDeviceControlModeMap.count(deviceName), 0) <<
- BOOST_CURRENT_FUNCTION << ": Must not request two ControlTargets for the same device. (got = "
- << controlDeviceControlModeMap.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 peekControlDevice(" + deviceName + ") and querry it)";
+ ARMARX_CHECK_EXPRESSION(NJointControllerRegistryEntry::ConstructorIsRunning())
+ << "The function '" << BOOST_CURRENT_FUNCTION
+ << "' must only be called during the construction of an NJointController.";
+ ARMARX_CHECK_EQUAL(controlDeviceControlModeMap.count(deviceName), 0)
+ << BOOST_CURRENT_FUNCTION
+ << ": Must not request two ControlTargets for the same device. (got = "
+ << controlDeviceControlModeMap.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 "
+ "peekControlDevice(" +
+ deviceName + ") and querry it)";
//there is a device and a target was requested:
- JointController* const jointCtrl = RobotUnitModule::DevicesAttorneyForNJointController::GetJointController(RobotUnitModule::Devices::Instance(), deviceName, controlMode);
+ JointController* const jointCtrl =
+ RobotUnitModule::DevicesAttorneyForNJointController::GetJointController(
+ RobotUnitModule::Devices::Instance(), deviceName, controlMode);
if (!jointCtrl)
{
return nullptr;
@@ -402,12 +441,11 @@ namespace armarx
ARMARX_CHECK_EQUAL(0, controlDeviceUsedBitmap.at(devIndex));
controlDeviceUsedBitmap.at(devIndex) = 1;
//we want this vector to be sorted
- controlDeviceUsedIndices.insert
- (
- std::upper_bound(controlDeviceUsedIndices.begin(), controlDeviceUsedIndices.end(), devIndex),
- devIndex
- );
+ controlDeviceUsedIndices.insert(std::upper_bound(controlDeviceUsedIndices.begin(),
+ controlDeviceUsedIndices.end(),
+ devIndex),
+ devIndex);
return jointCtrl->getControlTarget();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h
index d393f02ea5f90ff5ed41eab507fb6bf94cc7415a..3239c0e831c96536d442cba4531b2615a1bfe034 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h
@@ -22,28 +22,28 @@
#pragma once
-#include <RobotAPI/interface/units/RobotUnit/NJointController.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <atomic>
+#include <functional>
+#include <map>
+#include <mutex>
+#include <optional>
+
+#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h> // for ARMARX_CHECK
-#include <ArmarXCore/interface/core/ManagedIceObjectDefinitions.h>
#include <ArmarXCore/core/services/tasks/ThreadPool.h>
+#include <ArmarXCore/interface/core/ManagedIceObjectDefinitions.h>
-#include <VirtualRobot/VirtualRobot.h>
-
-#include <functional>
-#include <optional>
-#include <atomic>
-#include <mutex>
-#include <map>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointController.h>
namespace IceProxy::armarx
{
class DebugDrawerInterface;
class DebugObserverInterface;
class RobotUnitInterface;
-}
+} // namespace IceProxy::armarx
namespace armarx
{
@@ -52,18 +52,23 @@ namespace armarx
class NJointControllerAttorneyForPublisher;
class NJointControllerAttorneyForControlThread;
class NJointControllerAttorneyForControllerManagement;
- }
+ } // namespace RobotUnitModule
+
namespace detail
{
- template<class> class NJointControllerRegistryEntryHelper;
+ template <class>
+ class NJointControllerRegistryEntryHelper;
class ControlThreadOutputBufferEntry;
- }
+ } // namespace detail
+
namespace WidgetDescription
{
class Widget;
- typedef ::IceInternal::Handle< ::armarx::WidgetDescription::Widget> WidgetPtr;
- typedef ::std::map< ::std::string, ::armarx::WidgetDescription::WidgetPtr> StringWidgetDictionary;
- }
+ typedef ::IceInternal::Handle<::armarx::WidgetDescription::Widget> WidgetPtr;
+ typedef ::std::map<::std::string, ::armarx::WidgetDescription::WidgetPtr>
+ StringWidgetDictionary;
+ } // namespace WidgetDescription
+
using SensorAndControl = detail::ControlThreadOutputBufferEntry;
class JointController;
class SensorValueBase;
@@ -71,9 +76,12 @@ namespace armarx
using ConstControlDevicePtr = std::shared_ptr<const class ControlDevice>;
using ConstSensorDevicePtr = std::shared_ptr<const class SensorDevice>;
- typedef ::IceInternal::ProxyHandle< ::IceProxy::armarx::DebugDrawerInterface> DebugDrawerInterfacePrx;
- typedef ::IceInternal::ProxyHandle< ::IceProxy::armarx::DebugObserverInterface> DebugObserverInterfacePrx;
- typedef ::IceInternal::ProxyHandle< ::IceProxy::armarx::RobotUnitInterface> RobotUnitInterfacePrx;
+ typedef ::IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface>
+ DebugDrawerInterfacePrx;
+ typedef ::IceInternal::ProxyHandle<::IceProxy::armarx::DebugObserverInterface>
+ DebugObserverInterfacePrx;
+ typedef ::IceInternal::ProxyHandle<::IceProxy::armarx::RobotUnitInterface>
+ RobotUnitInterfacePrx;
using ThreadPoolPtr = std::shared_ptr<class ThreadPool>;
@@ -245,8 +253,8 @@ namespace armarx
* \subsection nj-example-1-h Header
* Include headers
* \code{.cpp}
- #include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+ #include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
* \endcode
*
@@ -341,8 +349,9 @@ namespace armarx
*
* Include the required headers.
* \code{.cpp}
- #include "NJointPositionPassThroughController.h"
#include <RobotAPI/libraries/core/Pose.h>
+
+ #include "NJointPositionPassThroughController.h"
* \endcode
*
* Open the namespace
@@ -566,7 +575,7 @@ namespace armarx
}
* \endcode
*/
- class NJointControllerBase:
+ class NJointControllerBase :
virtual public NJointControllerInterface,
virtual public ManagedIceObject
{
@@ -575,14 +584,13 @@ namespace armarx
// //////////////////////////////////////////////////////////////////////////////////////// //
public:
using ConfigPtrT = NJointControllerConfigPtr;
- using GenerateConfigDescriptionFunctionSignature = WidgetDescription::WidgetPtr(*)
- (
- const VirtualRobot::RobotPtr&,
- const std::map<std::string, ConstControlDevicePtr>& controlDevices,
- const std::map<std::string, ConstSensorDevicePtr>& sensorDevices
- );
- template<class ConfigPrtType>
- using GenerateConfigFromVariantsFunctionSignature = ConfigPrtType(*)(const StringVariantBaseMap&);
+ using GenerateConfigDescriptionFunctionSignature = WidgetDescription::WidgetPtr (*)(
+ const VirtualRobot::RobotPtr&,
+ const std::map<std::string, ConstControlDevicePtr>& controlDevices,
+ const std::map<std::string, ConstSensorDevicePtr>& sensorDevices);
+ template <class ConfigPrtType>
+ using GenerateConfigFromVariantsFunctionSignature =
+ ConfigPrtType (*)(const StringVariantBaseMap&);
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////// constructor setup functions ////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -593,7 +601,7 @@ namespace armarx
* @param deviceName The \ref SensorDevice's name
* @return A const ptr to the given \ref SensorDevice
*/
- ConstSensorDevicePtr peekSensorDevice(const std::string& deviceName) const;
+ ConstSensorDevicePtr peekSensorDevice(const std::string& deviceName) const;
/**
* @brief Get a const ptr to the given \ref ControlDevice
* \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
@@ -610,7 +618,9 @@ namespace armarx
* @param controlMode The \ref ControlTargetBase "ControlTarget's" ControlMode
* @return A ptr to the given \ref ControlDevice's \ref ControlTargetBase "ControlTarget"in the given ControlMode
*/
- ControlTargetBase* useControlTarget(const std::string& deviceName, const std::string& controlMode);
+ ControlTargetBase* useControlTarget(const std::string& deviceName,
+ const std::string& controlMode);
+
/**
* @brief Declares to calculate the \ref ControlTargetBase "ControlTarget"
* for the given \ref ControlDevice in the given ControlMode when \ref rtRun is called
@@ -619,8 +629,9 @@ namespace armarx
* @param controlMode The \ref ControlTargetBase "ControlTarget's" ControlMode
* @return A ptr to the given \ref ControlDevice's \ref ControlTargetBase "ControlTarget"in the given ControlMode
*/
- template<class T>
- T* useControlTarget(const std::string& deviceName, const std::string& controlMode)
+ template <class T>
+ T*
+ useControlTarget(const std::string& deviceName, const std::string& controlMode)
{
static_assert(std::is_base_of<ControlTargetBase, T>::value,
"The given type does not derive ControlTargetBase");
@@ -635,14 +646,16 @@ namespace armarx
* @return A const ptr to the given \ref SensorDevice's \ref SensorValueBase "SensorValue"
*/
const SensorValueBase* useSensorValue(const std::string& sensorDeviceName) const;
+
/**
* @brief Get a const ptr to the given \ref SensorDevice's \ref SensorValueBase "SensorValue"
* \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
* @param deviceName The \ref SensorDevice's name
* @return A const ptr to the given \ref SensorDevice's \ref SensorValueBase "SensorValue"
*/
- template<class T>
- const T* useSensorValue(const std::string& deviceName) const
+ template <class T>
+ const T*
+ useSensorValue(const std::string& deviceName) const
{
static_assert(std::is_base_of<SensorValueBase, T>::value,
"The given type does not derive SensorValueBase");
@@ -666,10 +679,12 @@ namespace armarx
// //////////////////////////////////////////////////////////////////////////////////////// //
protected:
/// @see Component::getDefaultName
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return getClassName();
}
+
/// @see Component::onInitComponent
void onInitComponent() final;
/// @see Component::onConnectComponent
@@ -679,11 +694,25 @@ namespace armarx
/// @see Component::onExitComponent
void onExitComponent() final;
+ virtual void
+ onInitNJointController()
+ {
+ }
+
+ virtual void
+ onConnectNJointController()
+ {
+ }
+
+ virtual void
+ onDisconnectNJointController()
+ {
+ }
- virtual void onInitNJointController() {}
- virtual void onConnectNJointController() {}
- virtual void onDisconnectNJointController() {}
- virtual void onExitNJointController() {}
+ virtual void
+ onExitNJointController()
+ {
+ }
// //////////////////////////////////////////////////////////////////////////////////////// //
// ////////////////////////////////// ThreadPool functionality///////////////////////////// //
@@ -719,18 +748,24 @@ namespace armarx
});
* @endcode
*/
- template < typename Task >
- void runTask(const std::string& taskName, Task&& task)
+ template <typename Task>
+ void
+ runTask(const std::string& taskName, Task&& task)
{
std::unique_lock lock(threadHandlesMutex);
ARMARX_CHECK_EXPRESSION(!taskName.empty());
ARMARX_CHECK_EXPRESSION(!threadHandles.count(taskName));
ARMARX_CHECK_EXPRESSION(getState() < eManagedIceObjectExiting);
- ARMARX_VERBOSE << "Adding NJointControllerBase task named '" << taskName << "' - current available thread count: " << getThreadPool()->getAvailableTaskCount();
+ ARMARX_VERBOSE << "Adding NJointControllerBase task named '" << taskName
+ << "' - current available thread count: "
+ << getThreadPool()->getAvailableTaskCount();
auto handlePtr = std::make_shared<ThreadPool::Handle>(getThreadPool()->runTask(task));
- ARMARX_CHECK_EXPRESSION(handlePtr->isValid()) << "Could not add task (" << taskName << " - " << GetTypeString(task) << " ) - available threads: " << getThreadPool()->getAvailableTaskCount();
+ ARMARX_CHECK_EXPRESSION(handlePtr->isValid())
+ << "Could not add task (" << taskName << " - " << GetTypeString(task)
+ << " ) - available threads: " << getThreadPool()->getAvailableTaskCount();
threadHandles[taskName] = handlePtr;
}
+
std::map<std::string, std::shared_ptr<ThreadPool::Handle>> threadHandles;
std::mutex threadHandlesMutex;
@@ -738,32 +773,44 @@ namespace armarx
// ///////////////////////////////////// ice interface //////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
public:
- bool isControllerActive(const Ice::Current& = Ice::emptyCurrent) const final override
+ bool
+ isControllerActive(const Ice::Current& = Ice::emptyCurrent) const final override
{
return isActive;
}
- bool isControllerRequested(const Ice::Current& = Ice::emptyCurrent) const final override
+
+ bool
+ isControllerRequested(const Ice::Current& = Ice::emptyCurrent) const final override
{
return isRequested;
}
- bool isDeletable(const Ice::Current& = Ice::emptyCurrent) const final override
+
+ bool
+ isDeletable(const Ice::Current& = Ice::emptyCurrent) const final override
{
return deletable;
}
- bool hasControllerError(const Ice::Current& = Ice::emptyCurrent) const final override
+
+ bool
+ hasControllerError(const Ice::Current& = Ice::emptyCurrent) const final override
{
return deactivatedBecauseOfError;
}
- std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override = 0;
- std::string getInstanceName(const Ice::Current& = Ice::emptyCurrent) const final override
+ std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override = 0;
+
+ std::string
+ getInstanceName(const Ice::Current& = Ice::emptyCurrent) const final override
{
return instanceName_;
}
- NJointControllerDescription getControllerDescription(const Ice::Current& = Ice::emptyCurrent) const final override;
- NJointControllerStatus getControllerStatus(const Ice::Current& = Ice::emptyCurrent) const final override;
- NJointControllerDescriptionWithStatus getControllerDescriptionWithStatus(const Ice::Current& = Ice::emptyCurrent) const final override;
+ NJointControllerDescription
+ getControllerDescription(const Ice::Current& = Ice::emptyCurrent) const final override;
+ NJointControllerStatus
+ getControllerStatus(const Ice::Current& = Ice::emptyCurrent) const final override;
+ NJointControllerDescriptionWithStatus getControllerDescriptionWithStatus(
+ const Ice::Current& = Ice::emptyCurrent) const final override;
//RobotUnitInterfacePrx getRobotUnit(const Ice::Current& = Ice::emptyCurrent) const final override;
void activateController(const Ice::Current& = Ice::emptyCurrent) final override;
@@ -771,39 +818,47 @@ namespace armarx
void deleteController(const Ice::Current& = Ice::emptyCurrent) final override;
void deactivateAndDeleteController(const Ice::Current& = Ice::emptyCurrent) final override;
- WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current& = Ice::emptyCurrent) const override
+ WidgetDescription::StringWidgetDictionary
+ getFunctionDescriptions(const Ice::Current& = Ice::emptyCurrent) const override
{
return {};
}
- void callDescribedFunction(const std::string&, const StringVariantBaseMap&, const Ice::Current& = Ice::emptyCurrent) override
- {
+ void
+ callDescribedFunction(const std::string&,
+ const StringVariantBaseMap&,
+ const Ice::Current& = Ice::emptyCurrent) override
+ {
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////////// rt interface ///////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
public: ///TODO make protected and use attorneys
-
/**
* @brief Returns the virtual robot used by this \ref NJointControllerBase in the \ref rtRun
* @return The virtual robot used by this \ref NJointControllerBase in the \ref rtRun
* @see useSynchronizedRtRobot
* @see rtGetRobotNodes
*/
- const VirtualRobot::RobotPtr& rtGetRobot()
+ const VirtualRobot::RobotPtr&
+ rtGetRobot()
{
return rtRobot;
}
+
/**
* @brief Returns the nodes of the virtual robot used by this \ref NJointControllerBase in the \ref rtRun
* @return The nodes of the virtual robot used by this \ref NJointControllerBase in the \ref rtRun
* @see useSynchronizedRtRobot
* @see rtGetRobot
*/
- const std::vector<VirtualRobot::RobotNodePtr>& rtGetRobotNodes()
+ const std::vector<VirtualRobot::RobotNodePtr>&
+ rtGetRobotNodes()
{
return rtRobotNodes;
}
+
/**
* @brief Returns whether this \ref NJointControllerBase calculates a \ref ControlTargetBase "ControlTarget"
* for the given \ref ControlDevice
@@ -811,7 +866,8 @@ namespace armarx
* @return Whether this \ref NJointControllerBase calculates a \ref ControlTargetBase "ControlTarget"
* for the given \ref ControlDevice
*/
- bool rtUsesControlDevice(std::size_t deviceIndex) const
+ bool
+ rtUsesControlDevice(std::size_t deviceIndex) const
{
return controlDeviceUsedBitmap.at(deviceIndex);
}
@@ -822,7 +878,8 @@ namespace armarx
* @return The indices of all \ref ControlDevice's this \ref NJointControllerBase
* calculates a \ref ControlTargetBase "ControlTarget" for.
*/
- const std::vector<std::size_t>& rtGetControlDeviceUsedIndices() const
+ const std::vector<std::size_t>&
+ rtGetControlDeviceUsedIndices() const
{
return controlDeviceUsedIndices;
}
@@ -830,7 +887,8 @@ namespace armarx
/**
* @brief Sets the error state to true. This will deactivate this controller!
*/
- bool rtGetErrorState() const
+ bool
+ rtGetErrorState() const
{
return errorState;
}
@@ -839,7 +897,8 @@ namespace armarx
* @brief Returns the number of used \ref ControlDevice "ControlDevices"
* @return The number of used \ref ControlDevice "ControlDevices"
*/
- std::size_t rtGetNumberOfUsedControlDevices() const
+ std::size_t
+ rtGetNumberOfUsedControlDevices() const
{
return controlDeviceUsedIndices.size();
}
@@ -848,7 +907,8 @@ namespace armarx
* @brief Returns the class name. (the c-string may be used for rt message logging)
* @return The class name. (the c-string may be used for rt message logging)
*/
- const std::string& rtGetClassName() const
+ const std::string&
+ rtGetClassName() const
{
return rtClassName_;
}
@@ -857,7 +917,8 @@ namespace armarx
* @brief Returns the instance name. (the c-string may be used for rt message logging)
* @return The instance name. (the c-string may be used for rt message logging)
*/
- const std::string& rtGetInstanceName() const
+ const std::string&
+ rtGetInstanceName() const
{
return instanceName_;
}
@@ -867,20 +928,29 @@ namespace armarx
* @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.
*/
- virtual void rtPreActivateController() {}
+ virtual void
+ rtPreActivateController()
+ {
+ }
+
/**
* @brief This function is called after the controller is deactivated.
* You can use it to deactivate a thread (DO NOT JOIN THREADS!) e.g. via a std::atomic_bool.
*/
- virtual void rtPostDeactivateController() {}
+ virtual void
+ rtPostDeactivateController()
+ {
+ }
/**
* @brief Sets the error state to true. This will deactivate this controller!
*/
- void rtSetErrorState()
+ void
+ rtSetErrorState()
{
errorState.store(true);
}
+
private:
/**
* @brief Activates this controller in the \ref RobotUnitModules::ControlThread "ControlThread".
@@ -914,7 +984,6 @@ namespace armarx
void rtDeactivateControllerBecauseOfError();
public:
-
static const NJointControllerBasePtr NullPtr;
NJointControllerBase();
@@ -928,33 +997,43 @@ namespace armarx
// //////////////////////////////////////////////////////////////////////////////////////// //
public:
//used control devices
- StringStringDictionary getControlDeviceUsedControlModeMap(const Ice::Current& = Ice::emptyCurrent) const final override
+ StringStringDictionary
+ getControlDeviceUsedControlModeMap(
+ const Ice::Current& = Ice::emptyCurrent) const final override
{
return controlDeviceControlModeMap;
}
- const std::vector<char>& getControlDeviceUsedBitmap() const
+
+ const std::vector<char>&
+ getControlDeviceUsedBitmap() const
{
return controlDeviceUsedBitmap;
}
- const std::vector<std::size_t>& getControlDeviceUsedIndices() const
+
+ const std::vector<std::size_t>&
+ getControlDeviceUsedIndices() const
{
return controlDeviceUsedIndices;
}
- const std::map<std::string, const JointController*>& getControlDevicesUsedJointController()
+ const std::map<std::string, const JointController*>&
+ getControlDevicesUsedJointController()
{
return controlDeviceUsedJointController;
}
//check for conflict
- std::optional<std::vector<char>> isNotInConflictWith(const NJointControllerBasePtr& other) const
+ std::optional<std::vector<char>>
+ isNotInConflictWith(const NJointControllerBasePtr& other) const
{
return isNotInConflictWith(other->getControlDeviceUsedBitmap());
}
+
std::optional<std::vector<char>> isNotInConflictWith(const std::vector<char>& used) const;
- template<class ItT>
- static std::optional<std::vector<char>> AreNotInConflict(ItT first, ItT last)
+ template <class ItT>
+ static std::optional<std::vector<char>>
+ AreNotInConflict(ItT first, ItT last)
{
if (first == last)
{
@@ -974,17 +1053,35 @@ namespace armarx
}
return inuse;
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ////////////////////////////////////// publishing ////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
protected:
//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&) {}
+ virtual void
+ onPublishActivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&)
+ {
+ }
+
+ virtual void
+ onPublishDeactivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&)
+ {
+ }
+
+ virtual void
+ onPublish(const SensorAndControl&,
+ const DebugDrawerInterfacePrx&,
+ const DebugObserverInterfacePrx&)
+ {
+ }
+
private:
- void publish(const SensorAndControl& sac, const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer);
- void deactivatePublish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer);
+ void publish(const SensorAndControl& sac,
+ const DebugDrawerInterfacePrx& draw,
+ const DebugObserverInterfacePrx& observer);
+ void deactivatePublish(const DebugDrawerInterfacePrx& draw,
+ const DebugObserverInterfacePrx& observer);
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////////////// data ///////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -1004,17 +1101,17 @@ namespace armarx
std::string instanceName_;
//this data is filled by the robot unit to provide convenience functions
- std::atomic_bool isActive {false};
- std::atomic_bool isRequested {false};
- std::atomic_bool deactivatedBecauseOfError {false};
- std::atomic_bool errorState {false};
- bool deletable {false};
- bool internal {false};
+ std::atomic_bool isActive{false};
+ std::atomic_bool isRequested{false};
+ std::atomic_bool deactivatedBecauseOfError{false};
+ std::atomic_bool errorState{false};
+ bool deletable{false};
+ bool internal{false};
- std::atomic_bool publishActive {false};
+ std::atomic_bool publishActive{false};
- std::atomic_bool statusReportedActive {false};
- std::atomic_bool statusReportedRequested {false};
+ std::atomic_bool statusReportedActive{false};
+ std::atomic_bool statusReportedRequested{false};
VirtualRobot::RobotPtr rtRobot;
std::vector<VirtualRobot::RobotNodePtr> rtRobotNodes;
@@ -1041,24 +1138,33 @@ namespace armarx
* \brief This is required for the factory
* \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
*/
- template<class> friend class detail::NJointControllerRegistryEntryHelper;
+ template <class>
+ friend class detail::NJointControllerRegistryEntryHelper;
};
class SynchronousNJointController : public NJointControllerBase
{
public: ///TODO make protected and use attorneys
- virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
- virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) = 0;
+
+ virtual void
+ rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
rtRun(sensorValuesTimestamp, timeSinceLastIteration);
}
};
+
class AsynchronousNJointController : public NJointControllerBase
{
public: ///TODO make protected and use attorneys
- virtual void rtRunIterationBegin(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
- virtual void rtRunIterationEnd(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
+ virtual void rtRunIterationBegin(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) = 0;
+ virtual void rtRunIterationEnd(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) = 0;
};
+
using NJointController = SynchronousNJointController;
using NJointControllerPtr = SynchronousNJointControllerPtr;
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h
index 2b49eb4dd9c039d24b13b6ccacbac05ac4f4a293..0fd471245b9a419a99c476f69d8da7a03bef24a7 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h
@@ -22,11 +22,11 @@
#pragma once
-#include "NJointControllerBase.h"
-
-#include <ArmarXCore/util/CPPUtility/TemplateMetaProgramming.h>
#include <ArmarXCore/core/util/OnScopeExit.h>
#include <ArmarXCore/core/util/Registrar.h>
+#include <ArmarXCore/util/CPPUtility/TemplateMetaProgramming.h>
+
+#include "NJointControllerBase.h"
namespace armarx::RobotUnitModule
{
@@ -60,12 +60,14 @@ namespace armarx
public:
virtual ~NJointControllerRegistryEntry() = default;
+
static bool
ConstructorIsRunning()
{
return ConstructorIsRunning_;
}
};
+
using NJointControllerRegistry = Registrar<std::unique_ptr<NJointControllerRegistryEntry>>;
template <class ControllerType>
@@ -80,7 +82,8 @@ namespace armarx
ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(
hasGenerateConfigFromVariants,
GenerateConfigFromVariants,
- NJointControllerBase::GenerateConfigFromVariantsFunctionSignature<typename T::ConfigPtrT>);
+ NJointControllerBase::GenerateConfigFromVariantsFunctionSignature<
+ typename T::ConfigPtrT>);
template <class NJointControllerT>
class NJointControllerRegistryEntryHelper : public NJointControllerRegistryEntry
@@ -130,7 +133,8 @@ namespace armarx
GenerateConfigDescription(
const VirtualRobot::RobotPtr& robot,
const std::map<std::string, ConstControlDevicePtr>& controlDevices,
- const std::map<std::string, ConstSensorDevicePtr>& sensorDevices) const final override
+ const std::map<std::string, ConstSensorDevicePtr>& sensorDevices)
+ const final override
{
if constexpr (hasRemoteConfiguration_)
{
@@ -221,6 +225,7 @@ namespace armarx
} // namespace detail
using NJointControllerRegistry = Registrar<std::unique_ptr<NJointControllerRegistryEntry>>;
+
template <class ControllerType>
struct NJointControllerRegistration
{
@@ -241,5 +246,3 @@ namespace armarx
static_assert( \
::armarx::detail::hasGenerateConfigFromVariants<T>::value, \
#T " does not offer a construction gui (missing: static GenerateConfigFromVariants)")
-
-
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h
index cd6befc54b45193149ce7106be84fb644599a8cf..04a4ef03727fded452307224464436b5d9cf5ddb 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h
@@ -1,9 +1,8 @@
#pragma once
-#include "NJointControllerBase.h"
-
#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
+#include "NJointControllerBase.h"
namespace armarx
{
@@ -15,29 +14,34 @@ namespace armarx
using LockGuardType = std::lock_guard<std::recursive_mutex>;
NJointControllerWithTripleBuffer(
- const ControlDataStruct& initialCommands = ControlDataStruct()) :
+ const ControlDataStruct& initialCommands = ControlDataStruct()) :
controlDataTripleBuffer{initialCommands}
{
}
- void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp,
- const IceUtil::Time& timeSinceLastIteration) override
+ void
+ rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override
{
rtUpdateControlStruct();
rtRun(sensorValuesTimestamp, timeSinceLastIteration);
}
protected:
- const ControlDataStruct& rtGetControlStruct() const
+ const ControlDataStruct&
+ rtGetControlStruct() const
{
return controlDataTripleBuffer.getReadBuffer();
}
- bool rtUpdateControlStruct()
+
+ bool
+ rtUpdateControlStruct()
{
return controlDataTripleBuffer.updateReadBuffer();
}
- void writeControlStruct()
+ void
+ writeControlStruct()
{
//just lock to be save and reduce the impact of an error
//also this allows code to unlock the mutex before calling this function
@@ -45,19 +49,23 @@ namespace armarx
LockGuardType lock(controlDataMutex);
controlDataTripleBuffer.commitWrite();
}
- ControlDataStruct& getWriterControlStruct()
+
+ ControlDataStruct&
+ getWriterControlStruct()
{
return controlDataTripleBuffer.getWriteBuffer();
}
- void setControlStruct(const ControlDataStruct& newStruct)
+ void
+ setControlStruct(const ControlDataStruct& newStruct)
{
LockGuardType lock{controlDataMutex};
getWriterControlStruct() = newStruct;
writeControlStruct();
}
- void reinitTripleBuffer(const ControlDataStruct& initial)
+ void
+ reinitTripleBuffer(const ControlDataStruct& initial)
{
controlDataTripleBuffer.reinitAllBuffers(initial);
}
@@ -70,6 +78,6 @@ namespace armarx
template <typename ControlDataStruct>
using NJointControllerWithTripleBufferPtr =
- IceInternal::Handle<NJointControllerWithTripleBuffer<ControlDataStruct>>;
+ IceInternal::Handle<NJointControllerWithTripleBuffer<ControlDataStruct>>;
} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
index 074efbc511936171b36d34acd0adbac87a1dde8a..8ce9b397c9147d041290f5f8f73c203d6b8cb326 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -22,33 +22,43 @@
* GNU General Public License
*/
#include "NJointHolonomicPlatformGlobalPositionController.h"
-#include "ArmarXCore/core/logging/Logging.h"
-#include "RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h"
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+#include <cmath>
#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
#include <SimoxUtility/math/periodic/periodic_clamp.h>
-#include <cmath>
+
+#include "ArmarXCore/core/logging/Logging.h"
+
+#include "RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h"
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
namespace armarx
{
NJointControllerRegistration<NJointHolonomicPlatformGlobalPositionController>
- registrationNJointHolonomicPlatformGlobalPositionController("NJointHolonomicPlatformGlobalPositionController");
-
-
- NJointHolonomicPlatformGlobalPositionController::NJointHolonomicPlatformGlobalPositionController(
- RobotUnit*,
- const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
- const VirtualRobot::RobotPtr&) :
+ registrationNJointHolonomicPlatformGlobalPositionController(
+ "NJointHolonomicPlatformGlobalPositionController");
+
+ NJointHolonomicPlatformGlobalPositionController::
+ NJointHolonomicPlatformGlobalPositionController(
+ RobotUnit*,
+ const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
+ const VirtualRobot::RobotPtr&) :
pid(cfg->p, cfg->i, cfg->d, cfg->maxVelocity, cfg->maxAcceleration),
- opid(cfg->p_rot, cfg->i_rot, cfg->d_rot, cfg->maxRotationVelocity, cfg->maxRotationAcceleration, true)
+ opid(cfg->p_rot,
+ cfg->i_rot,
+ cfg->d_rot,
+ cfg->maxRotationVelocity,
+ cfg->maxRotationAcceleration,
+ true)
{
- const SensorValueBase* sv = useSensorValue(GlobalRobotLocalizationSensorDevice::DeviceName());
+ const SensorValueBase* sv =
+ useSensorValue(GlobalRobotLocalizationSensorDevice::DeviceName());
this->sv = sv->asA<GlobalRobotLocalizationSensorDevice::SensorValueType>();
- target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)->asA<ControlTargetHolonomicPlatformVelocity>();
+ target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)
+ ->asA<ControlTargetHolonomicPlatformVelocity>();
pid.threadSafe = false;
@@ -57,13 +67,16 @@ namespace armarx
opid.threadSafe = false;
}
- void NJointHolonomicPlatformGlobalPositionController::rtRun(const IceUtil::Time& currentTime, const IceUtil::Time& timeSinceLastIteration)
+ void
+ NJointHolonomicPlatformGlobalPositionController::rtRun(
+ const IceUtil::Time& currentTime,
+ const IceUtil::Time& timeSinceLastIteration)
{
const auto global_T_robot = sv->global_T_root;
const Eigen::Vector3f rpy = simox::math::mat4f_to_rpy(global_T_robot);
const float global_orientation = rpy.z();
- const Eigen::Vector2f global_P_robot = global_T_robot.block<2,1>(0,3);
+ const Eigen::Vector2f global_P_robot = global_T_robot.block<2, 1>(0, 3);
if (rtGetControlStruct().newTargetSet)
{
@@ -72,7 +85,7 @@ namespace armarx
getWriterControlStruct().newTargetSet = false;
writeControlStruct();
-
+
isTargetSet = true;
}
@@ -99,7 +112,7 @@ namespace armarx
return;
}
- if(not sv->isAvailable())
+ if (not sv->isAvailable())
{
// ARMARX_RT_LOGF_INFO << deactivateSpam(1) << "global pose not available";
target->velocityX = 0;
@@ -111,8 +124,11 @@ namespace armarx
const float measuredOrientation = global_orientation;
- pid.update(timeSinceLastIteration.toSecondsDouble(), global_P_robot, rtGetControlStruct().target);
- opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(measuredOrientation), rtGetControlStruct().targetOrientation);
+ pid.update(
+ timeSinceLastIteration.toSecondsDouble(), global_P_robot, rtGetControlStruct().target);
+ opid.update(timeSinceLastIteration.toSecondsDouble(),
+ static_cast<double>(measuredOrientation),
+ rtGetControlStruct().targetOrientation);
const Eigen::Rotation2Df global_R_local(-measuredOrientation);
const Eigen::Vector2f velocities = global_R_local * pid.getControlValue();
@@ -122,25 +138,32 @@ namespace armarx
target->velocityRotation = static_cast<float>(opid.getControlValue());
}
- void NJointHolonomicPlatformGlobalPositionController::rtPreActivateController()
+ void
+ NJointHolonomicPlatformGlobalPositionController::rtPreActivateController()
{
target->velocityX = 0;
target->velocityY = 0;
target->velocityRotation = 0;
}
- void NJointHolonomicPlatformGlobalPositionController::setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy)
+ void
+ NJointHolonomicPlatformGlobalPositionController::setTarget(float x,
+ float y,
+ float yaw,
+ float translationAccuracy,
+ float rotationAccuracy)
{
// todo do we really need a recursive mutex?
std::lock_guard<std::recursive_mutex> lock(controlDataMutex);
getWriterControlStruct().target << x, y;
- getWriterControlStruct().targetOrientation = simox::math::periodic_clamp(yaw, -M_PIf32, M_PIf32);
+ getWriterControlStruct().targetOrientation =
+ simox::math::periodic_clamp(yaw, -M_PIf32, M_PIf32);
getWriterControlStruct().translationAccuracy = translationAccuracy;
getWriterControlStruct().rotationAccuracy = rotationAccuracy;
-
+
getWriterControlStruct().newTargetSet = true;
writeControlStruct();
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
index 37a0de6feb1720dc702b1b5e5dd356b36e974399..4e07bdb3e790f36875e2b080b1a1470b461eaa8b 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
@@ -30,22 +30,23 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "NJointControllerWithTripleBuffer.h"
-#include "../SensorValues/SensorValueHolonomicPlatform.h"
-
-#include "../ControlTargets/ControlTarget1DoFActuator.h"
#include "RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h"
-#include <RobotAPI/libraries/core/PIDController.h>
-
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h>
#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h>
#include <RobotAPI/interface/units/PlatformUnitInterface.h>
+#include <RobotAPI/libraries/core/PIDController.h>
+
+#include "../ControlTargets/ControlTarget1DoFActuator.h"
+#include "../SensorValues/SensorValueHolonomicPlatform.h"
+#include "NJointControllerWithTripleBuffer.h"
namespace armarx
{
TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformGlobalPositionControllerConfig);
- class NJointHolonomicPlatformGlobalPositionControllerConfig : virtual public NJointControllerConfig
+
+ class NJointHolonomicPlatformGlobalPositionControllerConfig :
+ virtual public NJointControllerConfig
{
public:
std::string platformName;
@@ -61,7 +62,6 @@ namespace armarx
float maxRotationVelocity = 1.0;
float maxRotationAcceleration = 0.5;
-
};
struct NJointHolonomicPlatformGlobalPositionControllerTarget
@@ -87,8 +87,9 @@ namespace armarx
* @brief The NJointHolonomicPlatformGlobalPositionController class
* @ingroup Library-RobotUnit-NJointControllers
*/
- class NJointHolonomicPlatformGlobalPositionController:
- virtual public NJointControllerWithTripleBuffer<NJointHolonomicPlatformGlobalPositionControllerTarget>
+ class NJointHolonomicPlatformGlobalPositionController :
+ virtual public NJointControllerWithTripleBuffer<
+ NJointHolonomicPlatformGlobalPositionControllerTarget>
{
public:
using ConfigPtrT = NJointHolonomicPlatformGlobalPositionControllerConfigPtr;
@@ -98,25 +99,27 @@ namespace armarx
const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
const VirtualRobot::RobotPtr&);
- inline virtual void rtRun(const IceUtil::Time&, const IceUtil::Time& timeSinceLastIteration) override;
+ inline virtual void rtRun(const IceUtil::Time&,
+ const IceUtil::Time& timeSinceLastIteration) override;
inline virtual void rtPreActivateController() override;
//ice interface
- inline virtual std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override
+ inline virtual std::string
+ getClassName(const Ice::Current& = Ice::emptyCurrent) const override
{
return "NJointHolonomicPlatformGlobalPositionController";
}
- void setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy);
+ void
+ setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy);
protected:
const GlobalRobotLocalizationSensorDevice::SensorValueType* sv;
MultiDimPIDController pid;
- PIDController opid;
+ PIDController opid;
ControlTargetHolonomicPlatformVelocity* target;
bool isTargetSet = false;
-
};
} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
index 40d37c7ae18892dfe18697ada68dcde1297e79ab..cca564afe0ad449f947c3e5be351f9b7ce7952ba 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
@@ -23,26 +23,30 @@
*/
#include "NJointHolonomicPlatformRelativePositionController.h"
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-
#include <Eigen/Geometry>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
namespace armarx
{
NJointControllerRegistration<NJointHolonomicPlatformRelativePositionController>
- registrationNJointHolonomicPlatformRelativePositionController("NJointHolonomicPlatformRelativePositionController");
-
-
- NJointHolonomicPlatformRelativePositionController::NJointHolonomicPlatformRelativePositionController(
- RobotUnit*,
- const NJointHolonomicPlatformRelativePositionControllerConfigPtr& cfg,
- const VirtualRobot::RobotPtr&) :
+ registrationNJointHolonomicPlatformRelativePositionController(
+ "NJointHolonomicPlatformRelativePositionController");
+
+ NJointHolonomicPlatformRelativePositionController::
+ NJointHolonomicPlatformRelativePositionController(
+ RobotUnit*,
+ const NJointHolonomicPlatformRelativePositionControllerConfigPtr& cfg,
+ const VirtualRobot::RobotPtr&) :
pid(cfg->p, cfg->i, cfg->d, cfg->maxVelocity, cfg->maxAcceleration)
{
const SensorValueBase* sv = useSensorValue(cfg->platformName);
this->sv = sv->asA<SensorValueHolonomicPlatform>();
- target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)->asA<ControlTargetHolonomicPlatformVelocity>();
- ARMARX_CHECK_EXPRESSION(target) << "The actuator '" << cfg->platformName << "' has no control mode " << ControlModes::HolonomicPlatformVelocity;
+ target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)
+ ->asA<ControlTargetHolonomicPlatformVelocity>();
+ ARMARX_CHECK_EXPRESSION(target)
+ << "The actuator '" << cfg->platformName << "' has no control mode "
+ << ControlModes::HolonomicPlatformVelocity;
pid.threadSafe = false;
@@ -56,7 +60,10 @@ namespace armarx
pid.preallocate(2);
}
- void NJointHolonomicPlatformRelativePositionController::rtRun(const IceUtil::Time&, const IceUtil::Time& timeSinceLastIteration)
+ void
+ NJointHolonomicPlatformRelativePositionController::rtRun(
+ const IceUtil::Time&,
+ const IceUtil::Time& timeSinceLastIteration)
{
currentPosition << sv->relativePositionX, sv->relativePositionY;
currentOrientation = sv->relativePositionRotation;
@@ -71,12 +78,16 @@ namespace armarx
//position pid
Eigen::Vector2f relativeCurrentPosition = currentPosition - startPosition;
- pid.update(timeSinceLastIteration.toSecondsDouble(), relativeCurrentPosition, rtGetControlStruct().target);
+ pid.update(timeSinceLastIteration.toSecondsDouble(),
+ relativeCurrentPosition,
+ rtGetControlStruct().target);
float relativeOrientation = currentOrientation - startOrientation;
//rotation pid
// Revert the rotation by rotating by the negative angle
- Eigen::Vector2f localTargetVelocity = Eigen::Rotation2Df(-currentOrientation) * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
+ Eigen::Vector2f localTargetVelocity =
+ Eigen::Rotation2Df(-currentOrientation) *
+ Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
//ARMARX_RT_LOGF_INFO("global target vel x: %.2f y: %2.f, local target vel x: %.2f y: %2.f rotation: %2.f", pid.getControlValue()[0], pid.getControlValue()[1], localTargetVelocity[0], localTargetVelocity[1], sv->relativePositionRotation).deactivateSpam(0.1);
target->velocityX = localTargetVelocity[0];
@@ -105,16 +116,22 @@ namespace armarx
// ARMARX_RT_LOGF_INFO("new target vel: %2.f, %2.f current vel: %2.f, %2.f", target->velocityX, target->velocityY, sv->velocityX, sv->velocityY).deactivateSpam(0.1);
}
- void NJointHolonomicPlatformRelativePositionController::rtPreActivateController()
+ void
+ NJointHolonomicPlatformRelativePositionController::rtPreActivateController()
{
startPosition[0] = sv->relativePositionX;
startPosition[1] = sv->relativePositionY;
startOrientation = sv->relativePositionRotation;
}
- void NJointHolonomicPlatformRelativePositionController::setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy)
+ void
+ NJointHolonomicPlatformRelativePositionController::setTarget(float x,
+ float y,
+ float yaw,
+ float translationAccuracy,
+ float rotationAccuracy)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().target << x, y;
getWriterControlStruct().targetOrientation = yaw;
getWriterControlStruct().translationAccuracy = translationAccuracy;
@@ -126,4 +143,3 @@ namespace armarx
} // namespace armarx
-
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
index 788a45607361a8fd3f8a36aeb54cbf9042416236..ad5b035266524a4079f84811ad8677279019f856 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
@@ -30,20 +30,21 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "NJointControllerWithTripleBuffer.h"
-#include "../SensorValues/SensorValueHolonomicPlatform.h"
-
-#include "../ControlTargets/ControlTarget1DoFActuator.h"
+#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h>
#include <RobotAPI/libraries/core/PIDController.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h>
+#include "../ControlTargets/ControlTarget1DoFActuator.h"
+#include "../SensorValues/SensorValueHolonomicPlatform.h"
+#include "NJointControllerWithTripleBuffer.h"
-#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
namespace armarx
{
TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformRelativePositionControllerConfig);
- class NJointHolonomicPlatformRelativePositionControllerConfig : virtual public NJointControllerConfig
+
+ class NJointHolonomicPlatformRelativePositionControllerConfig :
+ virtual public NJointControllerConfig
{
public:
std::string platformName;
@@ -72,8 +73,9 @@ namespace armarx
* @brief The NJointHolonomicPlatformRelativePositionController class
* @ingroup Library-RobotUnit-NJointControllers
*/
- class NJointHolonomicPlatformRelativePositionController:
- virtual public NJointControllerWithTripleBuffer<NJointHolonomicPlatformRelativePositionControllerTarget>
+ class NJointHolonomicPlatformRelativePositionController :
+ virtual public NJointControllerWithTripleBuffer<
+ NJointHolonomicPlatformRelativePositionControllerTarget>
{
public:
using ConfigPtrT = NJointHolonomicPlatformRelativePositionControllerConfigPtr;
@@ -83,16 +85,19 @@ namespace armarx
const NJointHolonomicPlatformRelativePositionControllerConfigPtr& cfg,
const VirtualRobot::RobotPtr&);
- inline virtual void rtRun(const IceUtil::Time&, const IceUtil::Time& timeSinceLastIteration) override;
+ inline virtual void rtRun(const IceUtil::Time&,
+ const IceUtil::Time& timeSinceLastIteration) override;
inline virtual void rtPreActivateController() override;
//ice interface
- inline virtual std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override
+ inline virtual std::string
+ getClassName(const Ice::Current& = Ice::emptyCurrent) const override
{
return "NJointHolonomicPlatformRelativePositionController";
}
- void setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy);
+ void
+ setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy);
protected:
@@ -106,5 +111,3 @@ namespace armarx
// float rad2MMFactor;
};
} // namespace armarx
-
-
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
index fa4b4ad337ed17d35b464a6485ecf45c81f63960..bd440c1b0b124795cbe519299bfca7931a64d3bf 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
@@ -22,65 +22,83 @@
#include "NJointHolonomicPlatformUnitVelocityPassThroughController.h"
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
namespace armarx
{
- NJointHolonomicPlatformUnitVelocityPassThroughController::NJointHolonomicPlatformUnitVelocityPassThroughController(
- RobotUnit* prov,
- NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr
- cfg, const VirtualRobot::RobotPtr&) :
+ NJointHolonomicPlatformUnitVelocityPassThroughController::
+ NJointHolonomicPlatformUnitVelocityPassThroughController(
+ RobotUnit* prov,
+ NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr cfg,
+ const VirtualRobot::RobotPtr&) :
maxCommandDelay(IceUtil::Time::milliSeconds(500))
{
- target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)->asA<ControlTargetHolonomicPlatformVelocity>();
- ARMARX_CHECK_EXPRESSION(target) << "The actuator " << cfg->platformName << " has no control mode " << ControlModes::HolonomicPlatformVelocity;
+ target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)
+ ->asA<ControlTargetHolonomicPlatformVelocity>();
+ ARMARX_CHECK_EXPRESSION(target)
+ << "The actuator " << cfg->platformName << " has no control mode "
+ << ControlModes::HolonomicPlatformVelocity;
- initialSettings.velocityX = cfg->initialVelocityX;
- initialSettings.velocityY = cfg->initialVelocityY;
+ initialSettings.velocityX = cfg->initialVelocityX;
+ initialSettings.velocityY = cfg->initialVelocityY;
initialSettings.velocityRotation = cfg->initialVelocityRotation;
reinitTripleBuffer(initialSettings);
}
- void NJointHolonomicPlatformUnitVelocityPassThroughController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time&)
+ void
+ NJointHolonomicPlatformUnitVelocityPassThroughController::rtRun(
+ const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time&)
{
auto commandAge = sensorValuesTimestamp - rtGetControlStruct().commandTimestamp;
- if (commandAge > maxCommandDelay && // command must be recent
- (rtGetControlStruct().velocityX != 0.0f || rtGetControlStruct().velocityY != 0.0f || rtGetControlStruct().velocityRotation != 0.0f)) // only throw error if any command is not zero
+ if (commandAge > maxCommandDelay && // command must be recent
+ (rtGetControlStruct().velocityX != 0.0f || rtGetControlStruct().velocityY != 0.0f ||
+ rtGetControlStruct().velocityRotation !=
+ 0.0f)) // only throw error if any command is not zero
{
- throw LocalException("Platform target velocity was not set for a too long time: delay: ") << commandAge.toSecondsDouble() << " s, max allowed delay: " << maxCommandDelay.toSecondsDouble() << " s";
+ throw LocalException(
+ "Platform target velocity was not set for a too long time: delay: ")
+ << commandAge.toSecondsDouble()
+ << " s, max allowed delay: " << maxCommandDelay.toSecondsDouble() << " s";
}
else
{
- target->velocityX = rtGetControlStruct().velocityX;
- target->velocityY = rtGetControlStruct().velocityY;
+ target->velocityX = rtGetControlStruct().velocityX;
+ target->velocityY = rtGetControlStruct().velocityY;
target->velocityRotation = rtGetControlStruct().velocityRotation;
}
}
- void NJointHolonomicPlatformUnitVelocityPassThroughController::setVelocites(float velocityX, float velocityY,
- float velocityRotation)
+ void
+ NJointHolonomicPlatformUnitVelocityPassThroughController::setVelocites(float velocityX,
+ float velocityY,
+ float velocityRotation)
{
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().velocityX = velocityX;
- getWriterControlStruct().velocityY = velocityY;
+ LockGuardType guard{controlDataMutex};
+ getWriterControlStruct().velocityX = velocityX;
+ getWriterControlStruct().velocityY = velocityY;
getWriterControlStruct().velocityRotation = velocityRotation;
getWriterControlStruct().commandTimestamp = IceUtil::Time::now();
writeControlStruct();
}
- IceUtil::Time NJointHolonomicPlatformUnitVelocityPassThroughController::getMaxCommandDelay() const
+ IceUtil::Time
+ NJointHolonomicPlatformUnitVelocityPassThroughController::getMaxCommandDelay() const
{
return maxCommandDelay;
}
- void NJointHolonomicPlatformUnitVelocityPassThroughController::setMaxCommandDelay(const IceUtil::Time& value)
+ void
+ NJointHolonomicPlatformUnitVelocityPassThroughController::setMaxCommandDelay(
+ const IceUtil::Time& value)
{
maxCommandDelay = value;
}
NJointControllerRegistration<NJointHolonomicPlatformUnitVelocityPassThroughController>
- registrationNJointHolonomicPlatformUnitVelocityPassThroughController("NJointHolonomicPlatformUnitVelocityPassThroughController");
-}
+ registrationNJointHolonomicPlatformUnitVelocityPassThroughController(
+ "NJointHolonomicPlatformUnitVelocityPassThroughController");
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
index e2c4a3aa4146a573c7fe4e92239272fd02d0372c..37df1f055d7daeb61630e62698585e484ed6cc75 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
@@ -24,15 +24,17 @@
#include <VirtualRobot/Robot.h>
-#include "NJointControllerWithTripleBuffer.h"
#include "../ControlTargets/ControlTargetHolonomicPlatformVelocity.h"
#include "../util.h"
+#include "NJointControllerWithTripleBuffer.h"
namespace armarx
{
TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformUnitVelocityPassThroughControllerConfig);
- class NJointHolonomicPlatformUnitVelocityPassThroughControllerConfig: virtual public NJointControllerConfig
+
+ class NJointHolonomicPlatformUnitVelocityPassThroughControllerConfig :
+ virtual public NJointControllerConfig
{
public:
std::string platformName;
@@ -42,6 +44,7 @@ namespace armarx
};
TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformUnitVelocityPassThroughController);
+
class NJointHolonomicPlatformUnitVelocityPassThroughControllerControlData
{
public:
@@ -52,12 +55,14 @@ namespace armarx
};
TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformUnitVelocityPassThroughController);
+
/**
* @brief The NJointHolonomicPlatformUnitVelocityPassThroughController class
* @ingroup Library-RobotUnit-NJointControllers
*/
- class NJointHolonomicPlatformUnitVelocityPassThroughController:
- virtual public NJointControllerWithTripleBuffer<NJointHolonomicPlatformUnitVelocityPassThroughControllerControlData>
+ class NJointHolonomicPlatformUnitVelocityPassThroughController :
+ virtual public NJointControllerWithTripleBuffer<
+ NJointHolonomicPlatformUnitVelocityPassThroughControllerControlData>
{
public:
using ConfigPtrT = NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr;
@@ -73,10 +78,12 @@ namespace armarx
void setVelocites(float velocityX, float velocityY, float velocityRotation);
//ice interface
- std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override
+ std::string
+ getClassName(const Ice::Current& = Ice::emptyCurrent) const override
{
return "NJointHolonomicPlatformUnitVelocityPassThroughController";
}
+
IceUtil::Time getMaxCommandDelay() const;
void setMaxCommandDelay(const IceUtil::Time& value);
@@ -86,4 +93,4 @@ namespace armarx
ControlTargetHolonomicPlatformVelocity* target;
NJointHolonomicPlatformUnitVelocityPassThroughControllerControlData initialSettings;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
index 0d8b30e6b30fc0ab29a6183de6f55a73e01b30c3..e51de4dbdb2c7b903d83aeb462d9b9fd8f6a695d 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
@@ -26,7 +26,9 @@
namespace armarx
{
- NJointControllerRegistration<NJointKinematicUnitPassThroughController> registrationControllerNJointKinematicUnitPassThroughController("NJointKinematicUnitPassThroughController");
+ NJointControllerRegistration<NJointKinematicUnitPassThroughController>
+ registrationControllerNJointKinematicUnitPassThroughController(
+ "NJointKinematicUnitPassThroughController");
NJointKinematicUnitPassThroughController::NJointKinematicUnitPassThroughController(
RobotUnit* prov,
@@ -72,7 +74,7 @@ namespace armarx
}
else
{
- throw InvalidArgumentException {"Unsupported control mode: " + cfg->controlMode};
+ throw InvalidArgumentException{"Unsupported control mode: " + cfg->controlMode};
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
index cbbb83f65750f6aa9e9d1de7d0f7c2e1abe3fdf4..ab280b2245ff23cecbb5062969168dca312abc2f 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
@@ -28,14 +28,14 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "NJointControllerBase.h"
-#include "../SensorValues/SensorValue1DoFActuator.h"
-
#include "../ControlTargets/ControlTarget1DoFActuator.h"
+#include "../SensorValues/SensorValue1DoFActuator.h"
+#include "NJointControllerBase.h"
namespace armarx
{
TYPEDEF_PTRS_HANDLE(NJointKinematicUnitPassThroughControllerConfig);
+
class NJointKinematicUnitPassThroughControllerConfig : virtual public NJointControllerConfig
{
public:
@@ -43,14 +43,13 @@ namespace armarx
std::string controlMode;
};
-
TYPEDEF_PTRS_HANDLE(NJointKinematicUnitPassThroughController);
+
/**
* @brief The NJointKinematicUnitPassThroughController class
* @ingroup Library-RobotUnit-NJointControllers
*/
- class NJointKinematicUnitPassThroughController:
- virtual public NJointController
+ class NJointKinematicUnitPassThroughController : virtual public NJointController
{
public:
using ConfigPtrT = NJointKinematicUnitPassThroughControllerConfigPtr;
@@ -60,16 +59,19 @@ namespace armarx
const NJointKinematicUnitPassThroughControllerConfigPtr& cfg,
const VirtualRobot::RobotPtr&);
- inline void rtRun(const IceUtil::Time&, const IceUtil::Time&) override
+ inline void
+ rtRun(const IceUtil::Time&, const IceUtil::Time&) override
{
target.setValue(control);
}
- inline void rtPreActivateController() override
- {
+ inline void
+ rtPreActivateController() override
+ {
}
- void reset()
+ void
+ reset()
{
control = (sensor.getValue()) * sensorToControlOnActivateFactor;
if (std::abs(control) < resetZeroThreshold)
@@ -79,23 +81,26 @@ namespace armarx
}
//ice interface
- inline std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override
+ inline std::string
+ getClassName(const Ice::Current& = Ice::emptyCurrent) const override
{
return "NJointKinematicUnitPassThroughController";
}
- void set(float val)
+ void
+ set(float val)
{
control = val;
}
protected:
- std::atomic<float> control {0};
+ std::atomic<float> control{0};
- template<bool ConstPtr>
+ template <bool ConstPtr>
struct ptr_wrapper
{
- float getValue() const
+ float
+ getValue() const
{
if (_float)
{
@@ -104,8 +109,9 @@ namespace armarx
return *_int16;
}
- template < bool constPtr = ConstPtr, std::enable_if_t < !constPtr, int > = 0 >
- void setValue(float val)
+ template <bool constPtr = ConstPtr, std::enable_if_t<!constPtr, int> = 0>
+ void
+ setValue(float val)
{
if (_float)
{
@@ -116,15 +122,15 @@ namespace armarx
*_int16 = static_cast<std::int16_t>(val);
}
}
- template<class T>
+ template <class T>
using maybe_const_ptr = std::conditional_t<ConstPtr, const T*, T*>;
- maybe_const_ptr<float> _float{nullptr};
+ maybe_const_ptr<float> _float{nullptr};
maybe_const_ptr<std::int16_t> _int16{nullptr};
};
ptr_wrapper<false> target;
ptr_wrapper<true> sensor;
- float sensorToControlOnActivateFactor {0};
- float resetZeroThreshold {0};
+ float sensorToControlOnActivateFactor{0};
+ float resetZeroThreshold{0};
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
index fcedf27a6a87d533ce5cd67a23cd7c9d36e7774b..12ca8b5a1ec6146599034643989c0a668704d888 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
@@ -22,11 +22,12 @@
* GNU General Public License
*/
#include "NJointTCPController.h"
-#include "../RobotUnit.h"
+
+#include <VirtualRobot/RobotNodeSet.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-#include <VirtualRobot/RobotNodeSet.h>
+#include "../RobotUnit.h"
#define DEFAULT_TCP_STRING "default TCP"
@@ -34,19 +35,24 @@
namespace armarx
{
- NJointControllerRegistration<NJointTCPController> registrationControllerNJointTCPController("NJointTCPController");
+ NJointControllerRegistration<NJointTCPController>
+ registrationControllerNJointTCPController("NJointTCPController");
- std::string NJointTCPController::getClassName(const Ice::Current&) const
+ std::string
+ NJointTCPController::getClassName(const Ice::Current&) const
{
return "NJointTCPController";
}
- void NJointTCPController::rtPreActivateController()
+ void
+ NJointTCPController::rtPreActivateController()
{
reinitTripleBuffer(NJointTCPControllerControlData());
}
- void NJointTCPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ NJointTCPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
auto mode = rtGetControlStruct().mode;
@@ -54,7 +60,9 @@ namespace armarx
if (mode == VirtualRobot::IKSolver::All)
{
x.resize(6);
- x << rtGetControlStruct().xVel, rtGetControlStruct().yVel, rtGetControlStruct().zVel, rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel, rtGetControlStruct().yawVel;
+ x << rtGetControlStruct().xVel, rtGetControlStruct().yVel, rtGetControlStruct().zVel,
+ rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel,
+ rtGetControlStruct().yawVel;
}
else if (mode == VirtualRobot::IKSolver::Position)
{
@@ -64,7 +72,8 @@ namespace armarx
else if (mode == VirtualRobot::IKSolver::Orientation)
{
x.resize(3);
- x << rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel, rtGetControlStruct().yawVel;
+ x << rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel,
+ rtGetControlStruct().yawVel;
}
else
{
@@ -81,12 +90,12 @@ namespace armarx
}
}
-
- ::armarx::WidgetDescription::WidgetSeq NJointTCPController::createSliders()
+ ::armarx::WidgetDescription::WidgetSeq
+ NJointTCPController::createSliders()
{
using namespace armarx::WidgetDescription;
::armarx::WidgetDescription::WidgetSeq widgets;
- auto addSlider = [&](const std::string & label, float limit)
+ auto addSlider = [&](const std::string& label, float limit)
{
widgets.emplace_back(new Label(false, label));
{
@@ -108,7 +117,11 @@ namespace armarx
return widgets;
}
- WidgetDescription::WidgetPtr NJointTCPController::GenerateConfigDescription(const VirtualRobot::RobotPtr& robot, const std::map<std::string, ConstControlDevicePtr>& controlDevices, const std::map<std::string, ConstSensorDevicePtr>&)
+ WidgetDescription::WidgetPtr
+ NJointTCPController::GenerateConfigDescription(
+ const VirtualRobot::RobotPtr& robot,
+ const std::map<std::string, ConstControlDevicePtr>& controlDevices,
+ const std::map<std::string, ConstSensorDevicePtr>&)
{
using namespace armarx::WidgetDescription;
HBoxLayoutPtr layout = new HBoxLayout;
@@ -153,12 +166,16 @@ namespace armarx
return layout;
}
- NJointTCPControllerConfigPtr NJointTCPController::GenerateConfigFromVariants(const StringVariantBaseMap& values)
+ NJointTCPControllerConfigPtr
+ NJointTCPController::GenerateConfigFromVariants(const StringVariantBaseMap& values)
{
- return new NJointTCPControllerConfig {values.at("nodeSetName")->getString(), values.at("tcpName")->getString()};
+ return new NJointTCPControllerConfig{values.at("nodeSetName")->getString(),
+ values.at("tcpName")->getString()};
}
- NJointTCPController::NJointTCPController(RobotUnit* robotUnit, const NJointTCPControllerConfigPtr& config, const VirtualRobot::RobotPtr& r)
+ NJointTCPController::NJointTCPController(RobotUnit* robotUnit,
+ const NJointTCPControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr& r)
{
ARMARX_CHECK_EXPRESSION(robotUnit);
ARMARX_CHECK_EXPRESSION(!config->nodeSetName.empty());
@@ -173,16 +190,26 @@ namespace armarx
targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
sensors.push_back(sv->asA<SensorValue1DoFActuatorPosition>());
};
- ik.reset(new VirtualRobot::DifferentialIK(nodeset, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
- tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING) ? nodeset->getTCP() : rtGetRobot()->getRobotNode(config->tcpName);
+ ik.reset(new VirtualRobot::DifferentialIK(
+ nodeset, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING)
+ ? nodeset->getTCP()
+ : rtGetRobot()->getRobotNode(config->tcpName);
ARMARX_CHECK_EXPRESSION(tcp) << config->tcpName;
nodeSetName = config->nodeSetName;
}
- void NJointTCPController::setVelocities(float xVel, float yVel, float zVel, float rollVel, float pitchVel, float yawVel, VirtualRobot::IKSolver::CartesianSelection mode)
+ void
+ NJointTCPController::setVelocities(float xVel,
+ float yVel,
+ float zVel,
+ float rollVel,
+ float pitchVel,
+ float yawVel,
+ VirtualRobot::IKSolver::CartesianSelection mode)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().xVel = xVel;
getWriterControlStruct().yVel = yVel;
getWriterControlStruct().zVel = zVel;
@@ -193,32 +220,35 @@ namespace armarx
writeControlStruct();
}
- std::string NJointTCPController::getNodeSetName() const
+ std::string
+ NJointTCPController::getNodeSetName() const
{
return nodeSetName;
}
- void NJointTCPController::rtPostDeactivateController()
+ void
+ NJointTCPController::rtPostDeactivateController()
{
-
}
- WidgetDescription::StringWidgetDictionary NJointTCPController::getFunctionDescriptions(const Ice::Current&) const
+ WidgetDescription::StringWidgetDictionary
+ NJointTCPController::getFunctionDescriptions(const Ice::Current&) const
{
using namespace armarx::WidgetDescription;
HBoxLayoutPtr layout = new HBoxLayout;
auto sliders = createSliders();
- layout->children.insert(layout->children.end(),
- sliders.begin(),
- sliders.end());
+ layout->children.insert(layout->children.end(), sliders.begin(), sliders.end());
return {{"ControllerTarget", layout}};
}
- void NJointTCPController::callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&)
+ void
+ NJointTCPController::callDescribedFunction(const std::string& name,
+ const StringVariantBaseMap& valueMap,
+ const Ice::Current&)
{
if (name == "ControllerTarget")
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().xVel = valueMap.at("x")->getFloat();
getWriterControlStruct().yVel = valueMap.at("y")->getFloat();
getWriterControlStruct().zVel = valueMap.at("z")->getFloat();
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
index 41a93a0c368288536ca2eca179b8532fe1d6e01a..ecf1fc2666dded6b34c9f46850d1b00f7b0c09de 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
@@ -24,11 +24,12 @@
#pragma once
-#include "NJointControllerWithTripleBuffer.h"
+#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Robot.h>
+
#include "../ControlTargets/ControlTarget1DoFActuator.h"
#include "../SensorValues/SensorValue1DoFActuator.h"
-#include <VirtualRobot/IK/DifferentialIK.h>
+#include "NJointControllerWithTripleBuffer.h"
namespace armarx
{
@@ -39,16 +40,17 @@ namespace armarx
class NJointTCPControllerConfig : virtual public NJointControllerConfig
{
public:
- NJointTCPControllerConfig(std::string const& nodeSetName, const std::string& tcpName):
- nodeSetName(nodeSetName),
- tcpName(tcpName)
- {}
+ NJointTCPControllerConfig(std::string const& nodeSetName, const std::string& tcpName) :
+ nodeSetName(nodeSetName), tcpName(tcpName)
+ {
+ }
const std::string nodeSetName;
const std::string tcpName;
};
TYPEDEF_PTRS_HANDLE(NJointTCPControllerControlData);
+
class NJointTCPControllerControlData
{
public:
@@ -61,7 +63,6 @@ namespace armarx
VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All;
};
-
/**
* @brief The NJointTCPController class
* @ingroup Library-RobotUnit-NJointControllers
@@ -71,29 +72,44 @@ namespace armarx
{
public:
using ConfigPtrT = NJointTCPControllerConfigPtr;
- NJointTCPController(RobotUnit* prov, const NJointTCPControllerConfigPtr& config, const VirtualRobot::RobotPtr& r);
+ NJointTCPController(RobotUnit* prov,
+ const NJointTCPControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr& r);
// NJointControllerInterface interface
std::string getClassName(const Ice::Current&) const override;
- WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current&) const override;
- void callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&) override;
+ WidgetDescription::StringWidgetDictionary
+ getFunctionDescriptions(const Ice::Current&) const override;
+ void callDescribedFunction(const std::string& name,
+ const StringVariantBaseMap& valueMap,
+ const Ice::Current&) override;
// NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
- static WidgetDescription::WidgetPtr GenerateConfigDescription(
- const VirtualRobot::RobotPtr&,
- const std::map<std::string, ConstControlDevicePtr>&,
- const std::map<std::string, ConstSensorDevicePtr>&);
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
+ static WidgetDescription::WidgetPtr
+ GenerateConfigDescription(const VirtualRobot::RobotPtr&,
+ const std::map<std::string, ConstControlDevicePtr>&,
+ const std::map<std::string, ConstSensorDevicePtr>&);
- static NJointTCPControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
+ static NJointTCPControllerConfigPtr
+ GenerateConfigFromVariants(const StringVariantBaseMap& values);
// for TCPControlUnit
- void setVelocities(float xVel, float yVel, float zVel, float rollVel, float pitchVel, float yawVel, VirtualRobot::IKSolver::CartesianSelection mode);
+ void setVelocities(float xVel,
+ float yVel,
+ float zVel,
+ float rollVel,
+ float pitchVel,
+ float yawVel,
+ VirtualRobot::IKSolver::CartesianSelection mode);
std::string getNodeSetName() const;
static ::armarx::WidgetDescription::WidgetSeq createSliders();
+
protected:
void rtPreActivateController() override;
void rtPostDeactivateController() override;
+
private:
std::vector<ControlTarget1DoFActuatorVelocity*> targets;
std::vector<const SensorValue1DoFActuatorPosition*> sensors;
@@ -105,4 +121,3 @@ namespace armarx
};
} // namespace armarx
-
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
index 124d66adbc8b24f176cdb7d6acf5df4d3815eccc..ee59658b4c78c60e3187d4e4a4c1db21a0bf25d7 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
@@ -22,19 +22,24 @@
#include "NJointTaskSpaceImpedanceController.h"
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
using namespace armarx;
-NJointControllerRegistration<NJointTaskSpaceImpedanceController> registrationControllerDSRTController("NJointTaskSpaceImpedanceController");
+NJointControllerRegistration<NJointTaskSpaceImpedanceController>
+ registrationControllerDSRTController("NJointTaskSpaceImpedanceController");
-NJointTaskSpaceImpedanceController::NJointTaskSpaceImpedanceController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+NJointTaskSpaceImpedanceController::NJointTaskSpaceImpedanceController(
+ const RobotUnitPtr& robotUnit,
+ const NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&)
{
- NJointTaskSpaceImpedanceControlConfigPtr cfg = NJointTaskSpaceImpedanceControlConfigPtr::dynamicCast(config);
+ NJointTaskSpaceImpedanceControlConfigPtr cfg =
+ NJointTaskSpaceImpedanceControlConfigPtr::dynamicCast(config);
ARMARX_CHECK_EXPRESSION(cfg);
ARMARX_CHECK_EXPRESSION(robotUnit);
@@ -56,9 +61,12 @@ NJointTaskSpaceImpedanceController::NJointTaskSpaceImpedanceController(const Rob
ARMARX_CHECK_EXPRESSION(casted_ct);
targets.push_back(casted_ct);
- const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+ const SensorValue1DoFActuatorTorque* torqueSensor =
+ sv->asA<SensorValue1DoFActuatorTorque>();
+ const SensorValue1DoFActuatorVelocity* velocitySensor =
+ sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorPosition* positionSensor =
+ sv->asA<SensorValue1DoFActuatorPosition>();
if (!torqueSensor)
{
ARMARX_WARNING << "No Torque sensor available for " << jointName;
@@ -89,15 +97,13 @@ NJointTaskSpaceImpedanceController::NJointTaskSpaceImpedanceController(const Rob
reinitTripleBuffer(initData);
}
-void NJointTaskSpaceImpedanceController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/, const IceUtil::Time& /*timeSinceLastIteration*/)
+void
+NJointTaskSpaceImpedanceController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& /*timeSinceLastIteration*/)
{
rtUpdateControlStruct();
- const auto& jointDesiredTorques = controller.run(
- rtGetControlStruct(),
- torqueSensors,
- velocitySensors,
- positionSensors
- );
+ const auto& jointDesiredTorques =
+ controller.run(rtGetControlStruct(), torqueSensors, velocitySensors, positionSensors);
ARMARX_CHECK_EQUAL(targets.size(), static_cast<size_t>(jointDesiredTorques.size()));
for (size_t i = 0; i < targets.size(); ++i)
@@ -118,10 +124,12 @@ void NJointTaskSpaceImpedanceController::rtRun(const IceUtil::Time& /*sensorValu
debugDataInfo.getWriteBuffer().quatError = controller.dbg.errorAngle;
debugDataInfo.getWriteBuffer().posiError = controller.dbg.posiError;
debugDataInfo.commitWrite();
-
}
-void NJointTaskSpaceImpedanceController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
+void
+NJointTaskSpaceImpedanceController::onPublish(const SensorAndControl&,
+ const DebugDrawerInterfacePrx&,
+ const DebugObserverInterfacePrx& debugObs)
{
StringVariantBaseMap datafields;
auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
@@ -130,55 +138,69 @@ void NJointTaskSpaceImpedanceController::onPublish(const SensorAndControl&, cons
datafields[pair.first] = new Variant(pair.second);
}
- datafields["desiredForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_x);
- datafields["desiredForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_y);
- datafields["desiredForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_z);
+ datafields["desiredForce_x"] =
+ new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_x);
+ datafields["desiredForce_y"] =
+ new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_y);
+ datafields["desiredForce_z"] =
+ new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_z);
- datafields["tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_x);
- datafields["tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_y);
- datafields["tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_z);
+ datafields["tcpDesiredTorque_x"] =
+ new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_x);
+ datafields["tcpDesiredTorque_y"] =
+ new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_y);
+ datafields["tcpDesiredTorque_z"] =
+ new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_z);
datafields["quatError"] = new Variant(debugDataInfo.getUpToDateReadBuffer().quatError);
datafields["posiError"] = new Variant(debugDataInfo.getUpToDateReadBuffer().posiError);
debugObs->setDebugChannel("DSControllerOutput", datafields);
-
-
}
-void NJointTaskSpaceImpedanceController::setPosition(const Eigen::Vector3f& target, const Ice::Current&)
+void
+NJointTaskSpaceImpedanceController::setPosition(const Eigen::Vector3f& target, const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().desiredPosition = target;
writeControlStruct();
}
-void NJointTaskSpaceImpedanceController::setOrientation(const Eigen::Quaternionf& target, const Ice::Current&)
+void
+NJointTaskSpaceImpedanceController::setOrientation(const Eigen::Quaternionf& target,
+ const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().desiredOrientation = target;
writeControlStruct();
}
-void NJointTaskSpaceImpedanceController::setPositionOrientation(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori, const Ice::Current&)
+void
+NJointTaskSpaceImpedanceController::setPositionOrientation(const Eigen::Vector3f& pos,
+ const Eigen::Quaternionf& ori,
+ const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().desiredPosition = pos;
getWriterControlStruct().desiredOrientation = ori;
writeControlStruct();
}
-void NJointTaskSpaceImpedanceController::setPose(const Eigen::Matrix4f& mat, const Ice::Current&)
+void
+NJointTaskSpaceImpedanceController::setPose(const Eigen::Matrix4f& mat, const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().desiredPosition = simox::math::mat4f_to_pos(mat);
getWriterControlStruct().desiredOrientation = simox::math::mat4f_to_quat(mat);
writeControlStruct();
}
-void NJointTaskSpaceImpedanceController::setImpedanceParameters(const std::string& name, const Ice::FloatSeq& vals, const Ice::Current&)
+void
+NJointTaskSpaceImpedanceController::setImpedanceParameters(const std::string& name,
+ const Ice::FloatSeq& vals,
+ const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
if (name == "Kpos")
{
ARMARX_CHECK_EQUAL(vals.size(), 3);
@@ -250,12 +272,13 @@ void NJointTaskSpaceImpedanceController::setImpedanceParameters(const std::strin
writeControlStruct();
}
-void NJointTaskSpaceImpedanceController::setNullspaceConfig(
- const Eigen::VectorXf& joint,
- const Eigen::VectorXf& knull,
- const Eigen::VectorXf& dnull, const Ice::Current&)
+void
+NJointTaskSpaceImpedanceController::setNullspaceConfig(const Eigen::VectorXf& joint,
+ const Eigen::VectorXf& knull,
+ const Eigen::VectorXf& dnull,
+ const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(joint.size()), targets.size());
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(knull.size()), targets.size());
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(dnull.size()), targets.size());
@@ -265,9 +288,12 @@ void NJointTaskSpaceImpedanceController::setNullspaceConfig(
writeControlStruct();
}
-void NJointTaskSpaceImpedanceController::setConfig(const NJointTaskSpaceImpedanceControlRuntimeConfig& cfg, const Ice::Current&)
+void
+NJointTaskSpaceImpedanceController::setConfig(
+ const NJointTaskSpaceImpedanceControlRuntimeConfig& cfg,
+ const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(cfg.Knull.size()), targets.size());
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(cfg.Dnull.size()), targets.size());
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(cfg.desiredJointPositions.size()), targets.size());
@@ -285,7 +311,7 @@ void NJointTaskSpaceImpedanceController::setConfig(const NJointTaskSpaceImpedanc
writeControlStruct();
}
-void armarx::NJointTaskSpaceImpedanceController::rtPreActivateController()
+void
+armarx::NJointTaskSpaceImpedanceController::rtPreActivateController()
{
-
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
index 06d725f856583029d4c99a781b4832a3ee4f3c80..42ce54179acab6ab646191155c259a8a05f1e38a 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
@@ -22,13 +22,13 @@
#pragma once
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-
#include <RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.h>
#include "../ControllerParts/CartesianImpedanceController.h"
@@ -53,28 +53,41 @@ namespace armarx
public:
using ConfigPtrT = NJointTaskSpaceImpedanceControlConfigPtr;
- NJointTaskSpaceImpedanceController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+ NJointTaskSpaceImpedanceController(const RobotUnitPtr& robotUnit,
+ const NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&);
-
- std::string getClassName(const Ice::Current&) const override
+ std::string
+ getClassName(const Ice::Current&) const override
{
return "TaskSpaceImpedanceController";
}
// NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
protected:
- void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
+ void onPublish(const SensorAndControl&,
+ const DebugDrawerInterfacePrx&,
+ const DebugObserverInterfacePrx&) override;
void setPosition(const Eigen::Vector3f&, const Ice::Current&) override;
void setOrientation(const Eigen::Quaternionf&, const Ice::Current&) override;
- void setPositionOrientation(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori, const Ice::Current&) override;
+ void setPositionOrientation(const Eigen::Vector3f& pos,
+ const Eigen::Quaternionf& ori,
+ const Ice::Current&) override;
void setPose(const Eigen::Matrix4f& mat, const Ice::Current&) override;
- void setImpedanceParameters(const std::string&, const Ice::FloatSeq&, const Ice::Current&) override;
- void setNullspaceConfig(const Eigen::VectorXf& joint, const Eigen::VectorXf& knull, const Eigen::VectorXf& dnull, const Ice::Current&) override;
- void setConfig(const NJointTaskSpaceImpedanceControlRuntimeConfig& cfg, const Ice::Current&) override;
+ void setImpedanceParameters(const std::string&,
+ const Ice::FloatSeq&,
+ const Ice::Current&) override;
+ void setNullspaceConfig(const Eigen::VectorXf& joint,
+ const Eigen::VectorXf& knull,
+ const Eigen::VectorXf& dnull,
+ const Ice::Current&) override;
+ void setConfig(const NJointTaskSpaceImpedanceControlRuntimeConfig& cfg,
+ const Ice::Current&) override;
private:
std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
@@ -108,6 +121,7 @@ namespace armarx
float quatError;
float posiError;
};
+
TripleBuffer<NJointTaskSpaceImpedanceControllerDebugInfo> debugDataInfo;
std::vector<std::string> jointNames;
@@ -118,4 +132,4 @@ namespace armarx
protected:
void rtPreActivateController() override;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
index 3b270bd0516b1e00dedfe3c6024c16ecbd6948bc..4e79acb8ff78a77e739ac8b2ee867ca8b64df701 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
@@ -1,19 +1,22 @@
#include "NJointTrajectoryController.h"
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-#include <RobotAPI/components/units/RobotUnit/util/RtLogging.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
+#include <VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.h>
-#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
-#include <VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+#include <RobotAPI/components/units/RobotUnit/util/RtLogging.h>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
namespace armarx
{
- NJointControllerRegistration<NJointTrajectoryController> registrationControllerNJointTrajectoryController("NJointTrajectoryController");
+ NJointControllerRegistration<NJointTrajectoryController>
+ registrationControllerNJointTrajectoryController("NJointTrajectoryController");
- NJointTrajectoryController::NJointTrajectoryController(RobotUnit* prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr& robot)
+ NJointTrajectoryController::NJointTrajectoryController(RobotUnit* prov,
+ const NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr& robot)
{
cfg = NJointTrajectoryControllerConfigPtr::dynamicCast(config);
ARMARX_CHECK_EXPRESSION(cfg) << "Needed type: NJointTrajectoryControllerConfigPtr";
@@ -47,27 +50,30 @@ namespace armarx
NJointTrajectoryController::~NJointTrajectoryController()
{
-
}
- std::string NJointTrajectoryController::getClassName(const Ice::Current&) const
+ std::string
+ NJointTrajectoryController::getClassName(const Ice::Current&) const
{
return "NJointTrajectoryController";
}
- void NJointTrajectoryController::onInitNJointController()
+ void
+ NJointTrajectoryController::onInitNJointController()
{
offeringTopic("StaticPlotter");
}
- void NJointTrajectoryController::onConnectNJointController()
+ void
+ NJointTrajectoryController::onConnectNJointController()
{
plotter = getTopic<StaticPlotterInterfacePrx>("StaticPlotter");
dbgObs = getTopic<DebugObserverInterfacePrx>("DebugObserver");
}
-
- void NJointTrajectoryController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ NJointTrajectoryController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
if (rtGetControlStruct().trajectoryCtrl)
{
@@ -75,13 +81,14 @@ namespace armarx
ARMARX_CHECK_EQUAL(sensors.size(), targets.size());
const auto& dimNames = contr.getTraj()->getDimensionNames();
for (size_t i = 0; i < sensors.size(); i++)
- // for(auto& s : sensors)
+ // for(auto& s : sensors)
{
ARMARX_CHECK_EQUAL(dimNames.at(i), cfg->jointNames.at(i));
currentPos(i) = sensors.at(i)->position;
}
- auto& newVelocities = contr.update(timeSinceLastIteration.toSecondsDouble() * direction, currentPos);
+ auto& newVelocities =
+ contr.update(timeSinceLastIteration.toSecondsDouble() * direction, currentPos);
// StringVariantBaseMap positions;
// ARMARX_RT_LOGF_INFO("Current error %.3f", contr.getCurrentError()(1)).deactivateSpam(0.1);
@@ -92,13 +99,16 @@ namespace armarx
// }
// dbgObs->setDebugChannel("positions", positions);
currentTimestamp = contr.getCurrentTimestamp();
- finished = (currentTimestamp >= contr.getTraj()->rbegin()->getTimestamp() && direction == 1.0)
- || (currentTimestamp <= contr.getTraj()->begin()->getTimestamp() && direction == -1.0);
+ finished =
+ (currentTimestamp >= contr.getTraj()->rbegin()->getTimestamp() &&
+ direction == 1.0) ||
+ (currentTimestamp <= contr.getTraj()->begin()->getTimestamp() && direction == -1.0);
for (size_t i = 0; i < targets.size(); ++i)
{
{
- targets.at(i)->velocity = (cfg->isPreview || finished) ? 0.0f : newVelocities(i);
+ targets.at(i)->velocity =
+ (cfg->isPreview || finished) ? 0.0f : newVelocities(i);
}
}
@@ -109,7 +119,8 @@ namespace armarx
}
}
- void NJointTrajectoryController::setTrajectory(const TrajectoryBasePtr& t, const Ice::Current&)
+ void
+ NJointTrajectoryController::setTrajectory(const TrajectoryBasePtr& t, const Ice::Current&)
{
TIMING_START(TrajectoryOptimization);
ARMARX_CHECK_EXPRESSION(t);
@@ -123,8 +134,10 @@ namespace armarx
{
std::list<Eigen::VectorXd> pathPoints;
double timeStep = cfg->preSamplingStepMs / 1000.0;
- Eigen::VectorXd maxVelocities = Eigen::VectorXd::Constant(trajectoryDimensions, cfg->maxVelocity);
- Eigen::VectorXd maxAccelerations = Eigen::VectorXd::Constant(trajectoryDimensions, cfg->maxAcceleration);
+ Eigen::VectorXd maxVelocities =
+ Eigen::VectorXd::Constant(trajectoryDimensions, cfg->maxVelocity);
+ Eigen::VectorXd maxAccelerations =
+ Eigen::VectorXd::Constant(trajectoryDimensions, cfg->maxAcceleration);
TrajectoryController::UnfoldLimitlessJointPositions(trajectory);
for (const Trajectory::TrajData& element : *trajectory)
{
@@ -137,7 +150,8 @@ namespace armarx
}
VirtualRobot::Path p(pathPoints, cfg->maxDeviation);
- VirtualRobot::TimeOptimalTrajectory timeOptimalTraj(p, maxVelocities, maxAccelerations, timeStep);
+ VirtualRobot::TimeOptimalTrajectory timeOptimalTraj(
+ p, maxVelocities, maxAccelerations, timeStep);
TrajectoryPtr newTraj = new Trajectory();
@@ -171,7 +185,6 @@ namespace armarx
derivs.push_back(timeOptimalTraj.getPosition(duration)[d]);
derivs.push_back(timeOptimalTraj.getVelocity(duration)[d]);
newTraj->addDerivationsToDimension(d, duration, derivs);
-
}
newTraj->setDimensionNames(trajectory->getDimensionNames());
newTraj->setLimitless(limitlessStates);
@@ -194,9 +207,11 @@ namespace armarx
for (size_t d = 0; d < trajectory->dim(); ++d)
{
auto positions = trajectory->getDimensionData(d, 0);
- posData[trajectory->getDimensionName(d)] = Ice::FloatSeq(positions.begin(), positions.end());
+ posData[trajectory->getDimensionName(d)] =
+ Ice::FloatSeq(positions.begin(), positions.end());
auto velocities = trajectory->getDimensionData(d, 1);
- velData[trajectory->getDimensionName(d)] = Ice::FloatSeq(velocities.begin(), velocities.end());
+ velData[trajectory->getDimensionName(d)] =
+ Ice::FloatSeq(velocities.begin(), velocities.end());
}
auto timestampsDouble = trajectory->getTimestamps();
Ice::FloatSeq timestamps(timestampsDouble.begin(), timestampsDouble.end());
@@ -209,32 +224,37 @@ namespace armarx
}
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
ARMARX_INFO << VAROUT(cfg->PID_p) << VAROUT(cfg->PID_i) << VAROUT(cfg->PID_d);
- trajectoryCtrl.reset(new TrajectoryController(trajectory, cfg->PID_p, cfg->PID_i, cfg->PID_d, false));
+ trajectoryCtrl.reset(
+ new TrajectoryController(trajectory, cfg->PID_p, cfg->PID_i, cfg->PID_d, false));
getWriterControlStruct().trajectoryCtrl = trajectoryCtrl;
writeControlStruct();
}
- bool NJointTrajectoryController::isFinished(const Ice::Current&)
+ bool
+ NJointTrajectoryController::isFinished(const Ice::Current&)
{
return finished;
}
- double NJointTrajectoryController::getCurrentTimestamp(const Ice::Current&)
+ double
+ NJointTrajectoryController::getCurrentTimestamp(const Ice::Current&)
{
return currentTimestamp;
}
- float NJointTrajectoryController::getCurrentProgressFraction(const Ice::Current&)
+ float
+ NJointTrajectoryController::getCurrentProgressFraction(const Ice::Current&)
{
return math::MathUtils::ILerp(startTimestamp, endTimestamp, currentTimestamp);
}
- double NJointTrajectoryController::getCurrentTrajTime() const
+ double
+ NJointTrajectoryController::getCurrentTrajTime() const
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
if (trajectoryCtrl)
{
return trajectoryCtrl->getCurrentTimestamp();
@@ -245,19 +265,22 @@ namespace armarx
}
}
- void NJointTrajectoryController::setLooping(bool looping)
+ void
+ NJointTrajectoryController::setLooping(bool looping)
{
this->looping = looping;
}
- double NJointTrajectoryController::getTrajEndTime() const
+ double
+ NJointTrajectoryController::getTrajEndTime() const
{
return trajEndTime;
}
- TrajectoryPtr NJointTrajectoryController::getTrajectoryCopy() const
+ TrajectoryPtr
+ NJointTrajectoryController::getTrajectoryCopy() const
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
if (trajectoryCtrl)
{
return TrajectoryPtr::dynamicCast(trajectoryCtrl->getTraj()->clone());
@@ -268,12 +291,14 @@ namespace armarx
}
}
- void NJointTrajectoryController::rtPreActivateController()
+ void
+ NJointTrajectoryController::rtPreActivateController()
{
startTime = IceUtil::Time();
}
- void NJointTrajectoryController::rtPostDeactivateController()
+ void
+ NJointTrajectoryController::rtPostDeactivateController()
{
for (auto& target : targets)
{
@@ -281,4 +306,4 @@ namespace armarx
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
index 072f02567b233234a6669cc1a0b4d366676d0759..eb13d7c90f16f4fe14ce3b70b26928f569aa8343 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
@@ -1,13 +1,15 @@
#pragma once
-#include "NJointControllerWithTripleBuffer.h"
#include <VirtualRobot/Robot.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.h>
-#include <RobotAPI/libraries/core/TrajectoryController.h>
+
+#include <ArmarXGui/interface/StaticPlotterInterface.h>
+
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <ArmarXGui/interface/StaticPlotterInterface.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.h>
+#include <RobotAPI/libraries/core/TrajectoryController.h>
+#include "NJointControllerWithTripleBuffer.h"
namespace armarx
{
@@ -28,7 +30,9 @@ namespace armarx
public NJointTrajectoryControllerInterface
{
public:
- NJointTrajectoryController(RobotUnit* prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+ NJointTrajectoryController(RobotUnit* prov,
+ const NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&);
~NJointTrajectoryController();
@@ -40,7 +44,8 @@ namespace armarx
// NJointController interface
void rtPreActivateController() override;
void rtPostDeactivateController() override;
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
// NJointTrajectoryControllerInterface interface
void setTrajectory(const TrajectoryBasePtr& t, const Ice::Current&) override;
@@ -52,6 +57,7 @@ namespace armarx
void setLooping(bool looping);
double getTrajEndTime() const;
TrajectoryPtr getTrajectoryCopy() const;
+
private:
IceUtil::Time startTime;
std::vector<ControlTarget1DoFActuatorVelocity*> targets;
@@ -70,7 +76,5 @@ namespace armarx
double currentTimestamp = 0;
double startTimestamp = 0, endTimestamp = 1;
-
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/PDController.h b/source/RobotAPI/components/units/RobotUnit/PDController.h
index 87abee65875d8827f49dbd1426ec20965cdb8ad8..4643f7724b1a13daa5c9aeca5a52d747b0fabf77 100644
--- a/source/RobotAPI/components/units/RobotUnit/PDController.h
+++ b/source/RobotAPI/components/units/RobotUnit/PDController.h
@@ -26,13 +26,15 @@
namespace armarx
{
- template<class FType>
+ template <class FType>
class PDController
{
public:
using FloatingType = FType;
- PDController(FloatingType p = 0, FloatingType d = 0): Kp {p}, Kd {d} {}
+ PDController(FloatingType p = 0, FloatingType d = 0) : Kp{p}, Kd{d}
+ {
+ }
// setters / getters
/// @see Kp
@@ -117,179 +119,215 @@ namespace armarx
private:
/// @brief p portion of the controller
- FloatingType Kp {0};
+ FloatingType Kp{0};
/// @brief d portion of the controller
- FloatingType Kd {0};
+ FloatingType Kd{0};
/// @brief High limit for the output value
- FloatingType limitOutHi {0};
+ FloatingType limitOutHi{0};
/// @brief Low limit for the output value
- FloatingType limitOutLo {0};
+ FloatingType limitOutLo{0};
/// @brief The target's radius.
/// if abs(error) <= targetRadius the error is 0,
/// otherwise the errors magnitude is reduced by targetRadius
- FloatingType targetRadius {0};
+ FloatingType targetRadius{0};
/// @brief The output can change maximally by this magnitude every time
- FloatingType outRampDelta {0};
+ FloatingType outRampDelta{0};
- FloatingType lastOutInterpolation {0};
+ FloatingType lastOutInterpolation{0};
/// @brief The error is clamped to -maxError, +maxError
- FloatingType maxError {0};
+ FloatingType maxError{0};
//controller state
/// @brief the number of control iterations
- std::uint64_t iterationCount {0};
+ std::uint64_t iterationCount{0};
/// @brief the last control target
- FloatingType lastTarget {0};
+ FloatingType lastTarget{0};
/// @brief the last control input value
- FloatingType lastInValue {0};
+ FloatingType lastInValue{0};
/// @brief the last control output value
- FloatingType lastOutValue {0};
+ FloatingType lastOutValue{0};
};
-}
+} // namespace armarx
//functions
namespace armarx
{
// setters / getters
- template<class FType>
- inline void PDController<FType>::setP(FType p)
+ template <class FType>
+ inline void
+ PDController<FType>::setP(FType p)
{
Kp = std::abs(p);
}
- template<class FType>
- inline FType PDController<FType>::getP() const
+
+ template <class FType>
+ inline FType
+ PDController<FType>::getP() const
{
return Kp;
}
- template<class FType>
- inline void PDController<FType>::setD(FType d)
+ template <class FType>
+ inline void
+ PDController<FType>::setD(FType d)
{
Kd = std::abs(d);
}
- template<class FType>
- inline FType PDController<FType>::getD() const
+
+ template <class FType>
+ inline FType
+ PDController<FType>::getD() const
{
return Kd;
}
- template<class FType>
- inline void PDController<FType>::limitOut(FType out)
+ template <class FType>
+ inline void
+ PDController<FType>::limitOut(FType out)
{
limitOut(-out, out);
}
- template<class FType>
- inline void PDController<FType>::limitOut(FType min, FType max)
+
+ template <class FType>
+ inline void
+ PDController<FType>::limitOut(FType min, FType max)
{
std::tie(limitOutLo, limitOutHi) = std::minmax(min, max);
}
- template<class FType>
- inline FType PDController<FType>::getLoOutLimit() const
+
+ template <class FType>
+ inline FType
+ PDController<FType>::getLoOutLimit() const
{
return limitOutLo;
}
- template<class FType>
- inline FType PDController<FType>::getHiOutLimit() const
+
+ template <class FType>
+ inline FType
+ PDController<FType>::getHiOutLimit() const
{
return limitOutHi;
}
- template<class FType>
- inline void PDController<FType>::setOutRampDelta(FType delta)
+ template <class FType>
+ inline void
+ PDController<FType>::setOutRampDelta(FType delta)
{
outRampDelta = delta;
}
- template<class FType>
- inline FType PDController<FType>::getOutRampDelta() const
+
+ template <class FType>
+ inline FType
+ PDController<FType>::getOutRampDelta() const
{
return outRampDelta;
}
- template<class FType>
- inline void PDController<FType>::setMaxError(FType error)
+ template <class FType>
+ inline void
+ PDController<FType>::setMaxError(FType error)
{
maxError = std::abs(error);
}
- template<class FType>
- inline FType PDController<FType>::getMaxError() const
+
+ template <class FType>
+ inline FType
+ PDController<FType>::getMaxError() const
{
return maxError;
}
- template<class FType>
- inline void PDController<FType>::setTargetRadius(FType radius)
+ template <class FType>
+ inline void
+ PDController<FType>::setTargetRadius(FType radius)
{
targetRadius = std::abs(radius);
}
- template<class FType>
- inline FType PDController<FType>::getTargetRadius() const
+
+ template <class FType>
+ inline FType
+ PDController<FType>::getTargetRadius() const
{
return targetRadius;
}
- template<class FType>
- inline void PDController<FType>::setLastOutInterpolation(FType lastOutFactor)
+ template <class FType>
+ inline void
+ PDController<FType>::setLastOutInterpolation(FType lastOutFactor)
{
if (!(0 >= lastOutFactor && lastOutFactor < 1))
{
- throw std::invalid_argument
- {
- "the interpolation factor has to be in [0, 1)! factor = " +
- to_string(lastOutFactor)
- };
+ throw std::invalid_argument{"the interpolation factor has to be in [0, 1)! factor = " +
+ to_string(lastOutFactor)};
}
lastOutInterpolation = lastOutFactor;
}
- template<class FType>
- inline FType PDController<FType>::getLastOutInterpolationFactor() const
+
+ template <class FType>
+ inline FType
+ PDController<FType>::getLastOutInterpolationFactor() const
{
return lastOutInterpolation;
}
//get controller state
- template<class FType>
- inline FType PDController<FType>::getLastTarget() const
+ template <class FType>
+ inline FType
+ PDController<FType>::getLastTarget() const
{
return lastTarget;
}
- template<class FType>
- inline FType PDController<FType>::getLastInValue() const
+
+ template <class FType>
+ inline FType
+ PDController<FType>::getLastInValue() const
{
return lastInValue;
}
- template<class FType>
- inline FType PDController<FType>::getLastOutValue() const
+
+ template <class FType>
+ inline FType
+ PDController<FType>::getLastOutValue() const
{
return lastOutValue;
}
- template<class FType>
- inline uint64_t PDController<FType>::getIterationCount() const
+
+ template <class FType>
+ inline uint64_t
+ PDController<FType>::getIterationCount() const
{
return iterationCount;
}
// updating functions
- template<class FType>
- inline void PDController<FType>::reset()
+ template <class FType>
+ inline void
+ PDController<FType>::reset()
{
iterationCount = 0;
}
- template<class FType>
- inline FType PDController<FType>::update()
+
+ template <class FType>
+ inline FType
+ PDController<FType>::update()
{
return update(lastInValue, lastTarget);
}
- template<class FType>
- inline FType PDController<FType>::update(FType inValue)
+
+ template <class FType>
+ inline FType
+ PDController<FType>::update(FType inValue)
{
return update(inValue, lastTarget);
}
- template<class FType>
- inline FType PDController<FType>::update(FType inValue, FType target)
+
+ template <class FType>
+ inline FType
+ PDController<FType>::update(FType inValue, FType target)
{
lastTarget = target;
@@ -337,4 +375,4 @@ namespace armarx
lastOutValue = out;
return out;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
index 17bdc45e13085d67407d061256319f526e4355c1..29f4b175adc0de5bbbca50aa92730d8871c31947 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
@@ -25,9 +25,8 @@
namespace armarx
{
-RobotUnit::~RobotUnit()
-{
-
-}
+ RobotUnit::~RobotUnit()
+ {
+ }
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
index 22ffd80aefe3191b42626aa0af7875a30188b14e..0c97bcd05f4e4d6f4231839b23a99bcec8131c0d 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
@@ -42,7 +42,7 @@ namespace armarx
* @ingroup Library-RobotUnit
* @brief
*/
- class RobotUnitPropertyDefinitions:
+ class RobotUnitPropertyDefinitions :
public RobotUnitModule::UnitsPropertyDefinitions,
public RobotUnitModule::DevicesPropertyDefinitions,
public RobotUnitModule::LoggingPropertyDefinitions,
@@ -53,7 +53,7 @@ namespace armarx
public RobotUnitModule::SelfCollisionCheckerPropertyDefinitions
{
public:
- RobotUnitPropertyDefinitions(std::string prefix):
+ RobotUnitPropertyDefinitions(std::string prefix) :
UnitsPropertyDefinitions(prefix),
DevicesPropertyDefinitions(prefix),
LoggingPropertyDefinitions(prefix),
@@ -62,9 +62,10 @@ namespace armarx
ManagementPropertyDefinitions(prefix),
ControlThreadPropertyDefinitions(prefix),
SelfCollisionCheckerPropertyDefinitions(prefix)
- {}
+ {
+ }
};
-}
+} // namespace armarx
namespace armarx
{
@@ -192,14 +193,17 @@ namespace armarx
public:
~RobotUnit();
- static RobotUnit& Instance()
+ static RobotUnit&
+ Instance()
{
return ModuleBase::Instance<RobotUnit>();
}
+
/// @see PropertyUser::createPropertyDefinitions()
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
+ armarx::PropertyDefinitionsPtr
+ createPropertyDefinitions() override
{
return PropertyDefinitionsPtr(new RobotUnitPropertyDefinitions(getConfigIdentifier()));
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
index 90910c9f28b246abac25b5b15f0c609bdf33fe3e..adc702a2c075ae5de545f1e9441bcd7a53d1d17a 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
@@ -20,18 +20,20 @@
* GNU General Public License
*/
+#include "RobotUnitModuleBase.h"
+
#include <sstream>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/util/Preprocessor.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
-#include "RobotUnitModuleBase.h"
#include "RobotUnitModules.h"
namespace armarx
{
- std::string to_string(RobotUnitState s)
+ std::string
+ to_string(RobotUnitState s)
{
switch (s)
{
@@ -48,70 +50,72 @@ namespace armarx
case armarx::RobotUnitState::Exiting:
return "RobotUnitState::Exiting";
}
- throw std::invalid_argument
- {
- "Unknown state " + to_string(static_cast<std::size_t>(s)) +
- "\nStacktrace\n" + LogSender::CreateBackTrace()
- };
+ throw std::invalid_argument{"Unknown state " + to_string(static_cast<std::size_t>(s)) +
+ "\nStacktrace\n" + LogSender::CreateBackTrace()};
}
-}
+} // namespace armarx
namespace armarx::RobotUnitModule
{
// //////////////////////////////////////////////////////////////////////////// //
// /////////////////////////// call for each module /////////////////////////// //
// //////////////////////////////////////////////////////////////////////////// //
-#define cast_to_and_call(Type, fn, rethrow) \
- ARMARX_TRACE; \
- try \
- { \
- dynamic_cast<Type*>(this)->Type::fn; \
- } \
- catch (Ice::Exception& e) \
- { \
- ARMARX_ERROR << "exception in " << #Type "::" #fn << "!\nwhat:\n" \
- << e.what() \
- << "\n\tid : " << e.ice_id() \
- << "\n\tfile : " << e.ice_file() \
- << "\n\tline : " << e.ice_line() \
- << "\n\tstack: " << e.ice_stackTrace(); \
- if(rethrow) { throw;} \
- } \
- catch (std::exception& e) \
- { \
- ARMARX_ERROR << "exception in " << #Type "::" #fn << "!\nwhat:\n" << e.what(); \
- if(rethrow) { throw;} \
- } \
- catch (...) \
- { \
- ARMARX_ERROR << "exception in " << #Type "::" #fn << "!"; \
- if(rethrow) { throw;} \
+#define cast_to_and_call(Type, fn, rethrow) \
+ ARMARX_TRACE; \
+ try \
+ { \
+ dynamic_cast<Type*>(this)->Type::fn; \
+ } \
+ catch (Ice::Exception & e) \
+ { \
+ ARMARX_ERROR << "exception in " << #Type "::" #fn << "!\nwhat:\n" \
+ << e.what() << "\n\tid : " << e.ice_id() << "\n\tfile : " << e.ice_file() \
+ << "\n\tline : " << e.ice_line() << "\n\tstack: " << e.ice_stackTrace(); \
+ if (rethrow) \
+ { \
+ throw; \
+ } \
+ } \
+ catch (std::exception & e) \
+ { \
+ ARMARX_ERROR << "exception in " << #Type "::" #fn << "!\nwhat:\n" << e.what(); \
+ if (rethrow) \
+ { \
+ throw; \
+ } \
+ } \
+ catch (...) \
+ { \
+ ARMARX_ERROR << "exception in " << #Type "::" #fn << "!"; \
+ if (rethrow) \
+ { \
+ throw; \
+ } \
}
#define for_each_module_apply(r, data, elem) data(elem)
-#define for_each_module(macro) \
+#define for_each_module(macro) \
BOOST_PP_SEQ_FOR_EACH(for_each_module_apply, macro, BOOST_PP_VARIADIC_TO_SEQ(RobotUnitModules))
-#define check_base(Type) \
- static_assert( \
- std::is_base_of<ModuleBase, Type>::value, \
- "The RobotUnitModule '" #Type "' has to derived ModuleBase" \
- );
+#define check_base(Type) \
+ static_assert(std::is_base_of<ModuleBase, Type>::value, \
+ "The RobotUnitModule '" #Type "' has to derived ModuleBase");
for_each_module(check_base)
#undef check_base
- void ModuleBase::checkDerivedClasses() const
+ void ModuleBase::checkDerivedClasses() const
{
-#define check_deriving(Type) \
- ARMARX_CHECK_NOT_NULL( \
- dynamic_cast<const Type*>(this)) << \
- "This class does not derive from " << GetTypeString<Type>();
+#define check_deriving(Type) \
+ ARMARX_CHECK_NOT_NULL(dynamic_cast<const Type*>(this)) \
+ << "This class does not derive from " << GetTypeString<Type>();
for_each_module(check_deriving)
#undef check_deriving
}
+
// //////////////////////////////////////////////////////////////////////////// //
// ManagedIceObject life cycle
- void ModuleBase::onInitComponent()
+ void
+ ModuleBase::onInitComponent()
{
ARMARX_TRACE;
checkDerivedClasses();
@@ -120,52 +124,61 @@ namespace armarx::RobotUnitModule
cast_to_and_call(::armarx::RobotUnitModule::RobotData, _initVirtualRobot(), true);
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnInitRobotUnit(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnInitRobotUnit(), true)
for_each_module(call_module_hook)
#undef call_module_hook
- onInitRobotUnit();
+ onInitRobotUnit();
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnInitRobotUnit(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnInitRobotUnit(), true)
for_each_module(call_module_hook)
#undef call_module_hook
- finishComponentInitialization();
-
+ finishComponentInitialization();
}
- void ModuleBase::onConnectComponent()
+ void
+ ModuleBase::onConnectComponent()
{
ARMARX_TRACE;
checkDerivedClasses();
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnConnectRobotUnit(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnConnectRobotUnit(), true)
for_each_module(call_module_hook)
#undef call_module_hook
- onConnectRobotUnit();
+ onConnectRobotUnit();
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnConnectRobotUnit(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnConnectRobotUnit(), true)
for_each_module(call_module_hook)
#undef call_module_hook
}
- void ModuleBase::onDisconnectComponent()
+
+ void
+ ModuleBase::onDisconnectComponent()
{
ARMARX_TRACE;
checkDerivedClasses();
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnDisconnectRobotUnit(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnDisconnectRobotUnit(), true)
for_each_module(call_module_hook)
#undef call_module_hook
- onDisconnectRobotUnit();
+ onDisconnectRobotUnit();
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnDisconnectRobotUnit(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnDisconnectRobotUnit(), true)
for_each_module(call_module_hook)
#undef call_module_hook
}
- void ModuleBase::onExitComponent()
+ void
+ ModuleBase::onExitComponent()
{
ARMARX_TRACE;
checkDerivedClasses();
@@ -173,122 +186,150 @@ namespace armarx::RobotUnitModule
finishRunning();
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnExitRobotUnit(), false)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnExitRobotUnit(), false)
for_each_module(call_module_hook)
#undef call_module_hook
- onExitRobotUnit();
+ onExitRobotUnit();
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnExitRobotUnit(), false)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnExitRobotUnit(), false)
for_each_module(call_module_hook)
#undef call_module_hook
}
+
// //////////////////////////////////////////////////////////////////////////// //
// other ManagedIceObject / Component methods
- void ModuleBase::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
+ void
+ ModuleBase::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
{
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _componentPropertiesUpdated(changedProperties), true)
+#define call_module_hook(Type) \
+ cast_to_and_call( \
+ ::armarx::RobotUnitModule::Type, _componentPropertiesUpdated(changedProperties), true)
for_each_module(call_module_hook)
#undef call_module_hook
}
- void ModuleBase::icePropertiesInitialized()
+
+ void
+ ModuleBase::icePropertiesInitialized()
{
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _icePropertiesInitialized(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _icePropertiesInitialized(), true)
for_each_module(call_module_hook)
#undef call_module_hook
}
+
// //////////////////////////////////////////////////////////////////////////// //
// RobotUnit life cycle
- void ModuleBase::finishComponentInitialization()
+ void
+ ModuleBase::finishComponentInitialization()
{
ARMARX_TRACE;
checkDerivedClasses();
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingComponent, __FUNCTION__);
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishComponentInitialization(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishComponentInitialization(), true)
for_each_module(call_module_hook)
#undef call_module_hook
- setRobotUnitState(RobotUnitState::InitializingDevices);
+ setRobotUnitState(RobotUnitState::InitializingDevices);
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishComponentInitialization(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishComponentInitialization(), true)
for_each_module(call_module_hook)
#undef call_module_hook
}
- void ModuleBase::finishDeviceInitialization()
+
+ void
+ ModuleBase::finishDeviceInitialization()
{
ARMARX_TRACE;
checkDerivedClasses();
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishDeviceInitialization(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishDeviceInitialization(), true)
for_each_module(call_module_hook)
#undef call_module_hook
- setRobotUnitState(RobotUnitState::InitializingUnits);
+ setRobotUnitState(RobotUnitState::InitializingUnits);
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishDeviceInitialization(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishDeviceInitialization(), true)
for_each_module(call_module_hook)
#undef call_module_hook
}
- void ModuleBase::finishUnitInitialization()
+ void
+ ModuleBase::finishUnitInitialization()
{
ARMARX_TRACE;
checkDerivedClasses();
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishUnitInitialization(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishUnitInitialization(), true)
for_each_module(call_module_hook)
#undef call_module_hook
- setRobotUnitState(RobotUnitState::InitializingControlThread);
+ setRobotUnitState(RobotUnitState::InitializingControlThread);
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishUnitInitialization(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishUnitInitialization(), true)
for_each_module(call_module_hook)
#undef call_module_hook
}
- void ModuleBase::finishControlThreadInitialization()
+ void
+ ModuleBase::finishControlThreadInitialization()
{
ARMARX_TRACE;
checkDerivedClasses();
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingControlThread, __FUNCTION__);
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishControlThreadInitialization(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishControlThreadInitialization(), true)
for_each_module(call_module_hook)
#undef call_module_hook
- setRobotUnitState(RobotUnitState::Running);
+ setRobotUnitState(RobotUnitState::Running);
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishControlThreadInitialization(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call( \
+ ::armarx::RobotUnitModule::Type, _postFinishControlThreadInitialization(), true)
for_each_module(call_module_hook)
#undef call_module_hook
}
- void ModuleBase::finishRunning()
+ void
+ ModuleBase::finishRunning()
{
ARMARX_TRACE;
checkDerivedClasses();
shutDown();
- GuardType guard {dataMutex}; //DO NOT USE getGuard here!
+ GuardType guard{dataMutex}; //DO NOT USE getGuard here!
if (getRobotUnitState() != RobotUnitState::Running)
{
- ARMARX_ERROR << "called finishRunning when state was " << to_string(getRobotUnitState());
+ ARMARX_ERROR << "called finishRunning when state was "
+ << to_string(getRobotUnitState());
}
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishRunning(), false)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishRunning(), false)
for_each_module(call_module_hook)
#undef call_module_hook
- setRobotUnitState(RobotUnitState::Exiting);
+ setRobotUnitState(RobotUnitState::Exiting);
joinControlThread();
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishRunning(), false)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishRunning(), false)
for_each_module(call_module_hook)
#undef call_module_hook
}
@@ -300,37 +341,37 @@ namespace armarx::RobotUnitModule
// ////////////////////////////////// other /////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////// //
- const std::set<RobotUnitState> ModuleBase::DevicesReadyStates
- {
+ const std::set<RobotUnitState> ModuleBase::DevicesReadyStates{
RobotUnitState::InitializingUnits,
RobotUnitState::InitializingControlThread,
- RobotUnitState::Running
- };
+ RobotUnitState::Running};
- void ModuleBase::setRobotUnitState(RobotUnitState to)
+ void
+ ModuleBase::setRobotUnitState(RobotUnitState to)
{
ARMARX_TRACE;
auto guard = getGuard();
- const auto transitionErrorMessage = [ =, this ](RobotUnitState expectedFrom)
+ const auto transitionErrorMessage = [=, this](RobotUnitState expectedFrom)
{
return "Can't switch to state " + to_string(to) + " from " + to_string(state) +
"! The expected source state is " + to_string(expectedFrom) + ".";
};
- const auto transitionErrorThrow = [ =, this ](RobotUnitState expectedFrom)
+ const auto transitionErrorThrow = [=, this](RobotUnitState expectedFrom)
{
if (state != expectedFrom)
{
const auto msg = transitionErrorMessage(expectedFrom);
ARMARX_ERROR << msg;
- throw std::invalid_argument {msg};
+ throw std::invalid_argument{msg};
}
};
switch (to)
{
case armarx::RobotUnitState::InitializingComponent:
- throw std::invalid_argument {"Can't switch to state RobotUnitState::InitializingComponent"};
+ throw std::invalid_argument{
+ "Can't switch to state RobotUnitState::InitializingComponent"};
case armarx::RobotUnitState::InitializingDevices:
transitionErrorThrow(RobotUnitState::InitializingComponent);
break;
@@ -350,71 +391,74 @@ namespace armarx::RobotUnitModule
}
break;
default:
- throw std::invalid_argument
- {
- "setRobotUnitState: Unknown target state " + to_string(static_cast<std::size_t>(to)) +
- "\nStacktrace\n" + LogSender::CreateBackTrace()
- };
+ throw std::invalid_argument{"setRobotUnitState: Unknown target state " +
+ to_string(static_cast<std::size_t>(to)) +
+ "\nStacktrace\n" + LogSender::CreateBackTrace()};
}
ARMARX_INFO << "switching state from " << to_string(state) << " to " << to_string(to);
state = to;
}
-
- bool ModuleBase::areDevicesReady() const
+ bool
+ ModuleBase::areDevicesReady() const
{
return DevicesReadyStates.count(state);
}
- bool ModuleBase::inControlThread() const
+ bool
+ ModuleBase::inControlThread() const
{
return getRobotUnitState() == RobotUnitState::Running &&
std::this_thread::get_id() == _module<ControlThread>().getControlThreadId();
}
- void ModuleBase::throwIfInControlThread(const std::string& fnc) const
+ void
+ ModuleBase::throwIfInControlThread(const std::string& fnc) const
{
ARMARX_TRACE;
if (inControlThread())
{
std::stringstream str;
- str << "Called function '" << fnc << "' in the Control Thread\nStack trace:\n" << LogSender::CreateBackTrace();
+ str << "Called function '" << fnc << "' in the Control Thread\nStack trace:\n"
+ << LogSender::CreateBackTrace();
ARMARX_ERROR << str.str();
- throw LogicError {str.str()};
+ throw LogicError{str.str()};
}
}
- ModuleBase::GuardType ModuleBase::getGuard() const
+ ModuleBase::GuardType
+ ModuleBase::getGuard() const
{
ARMARX_TRACE;
if (inControlThread())
{
- throw LogicError
- {
- "Attempted to lock mutex in Control Thread\nStack trace:\n" +
- LogSender::CreateBackTrace()
- };
+ throw LogicError{"Attempted to lock mutex in Control Thread\nStack trace:\n" +
+ LogSender::CreateBackTrace()};
}
- GuardType guard {dataMutex, std::defer_lock};
+ GuardType guard{dataMutex, std::defer_lock};
if (guard.try_lock())
{
return guard;
}
- while (!guard.try_lock_for(std::chrono::microseconds {100}))
+ while (!guard.try_lock_for(std::chrono::microseconds{100}))
{
if (isShuttingDown())
{
- throw LogicError {"Attempting to lock mutex during shutdown\nStacktrace\n" + LogSender::CreateBackTrace()};
+ throw LogicError{"Attempting to lock mutex during shutdown\nStacktrace\n" +
+ LogSender::CreateBackTrace()};
}
}
return guard;
}
- void ModuleBase::throwIfStateIsNot(const std::set<RobotUnitState>& stateSet, const std::string& fnc, bool onlyWarn) const
+ void
+ ModuleBase::throwIfStateIsNot(const std::set<RobotUnitState>& stateSet,
+ const std::string& fnc,
+ bool onlyWarn) const
{
ARMARX_TRACE;
- if (! stateSet.count(state))
+ if (!stateSet.count(state))
{
std::stringstream ss;
ss << fnc << ": Can't be called if state is not in {";
@@ -429,20 +473,26 @@ namespace armarx::RobotUnitModule
fst = false;
}
ss << "} (current state " << getRobotUnitState() << ")\n"
- << "Stacktrace:\n" << LogSender::CreateBackTrace();
+ << "Stacktrace:\n"
+ << LogSender::CreateBackTrace();
ARMARX_ERROR << ss.str();
if (!onlyWarn)
{
- throw LogicError {ss.str()};
+ throw LogicError{ss.str()};
}
}
}
- void ModuleBase::throwIfStateIsNot(RobotUnitState s, const std::string& fnc, bool onlyWarn) const
+
+ void
+ ModuleBase::throwIfStateIsNot(RobotUnitState s, const std::string& fnc, bool onlyWarn) const
{
- throwIfStateIsNot(std::set<RobotUnitState> {s}, fnc, onlyWarn);
+ throwIfStateIsNot(std::set<RobotUnitState>{s}, fnc, onlyWarn);
}
- void ModuleBase::throwIfStateIs(const std::set<RobotUnitState>& stateSet, const std::string& fnc, bool onlyWarn) const
+ void
+ ModuleBase::throwIfStateIs(const std::set<RobotUnitState>& stateSet,
+ const std::string& fnc,
+ bool onlyWarn) const
{
ARMARX_TRACE;
if (stateSet.count(state))
@@ -460,36 +510,40 @@ namespace armarx::RobotUnitModule
fst = false;
}
ss << "} (current state " << getRobotUnitState() << ")\n"
- << "Stacktrace:\n" << LogSender::CreateBackTrace();
+ << "Stacktrace:\n"
+ << LogSender::CreateBackTrace();
ARMARX_ERROR << ss.str();
if (!onlyWarn)
{
- throw LogicError {ss.str()};
+ throw LogicError{ss.str()};
}
}
}
- void ModuleBase::throwIfStateIs(RobotUnitState s, const std::string& fnc, bool onlyWarn) const
+
+ void
+ ModuleBase::throwIfStateIs(RobotUnitState s, const std::string& fnc, bool onlyWarn) const
{
- throwIfStateIs(std::set<RobotUnitState> {s}, fnc, onlyWarn);
+ throwIfStateIs(std::set<RobotUnitState>{s}, fnc, onlyWarn);
}
- void ModuleBase::throwIfDevicesNotReady(const std::string& fnc) const
+ void
+ ModuleBase::throwIfDevicesNotReady(const std::string& fnc) const
{
throwIfStateIsNot(DevicesReadyStates, fnc);
}
-
- std::atomic<ModuleBase*> ModuleBase::Instance_ {nullptr};
+ std::atomic<ModuleBase*> ModuleBase::Instance_{nullptr};
ModuleBase::ModuleBase()
{
ARMARX_TRACE;
if (Instance_ && this != Instance_)
{
- ARMARX_FATAL << "This class is a Singleton. It was already instantiated. (Instance = " << Instance_ << ", this = " << this << ")";
+ ARMARX_FATAL << "This class is a Singleton. It was already instantiated. (Instance = "
+ << Instance_ << ", this = " << this << ")";
std::terminate();
}
Instance_ = this;
}
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.h
index 474c2d611ba4d12b20cd402851b3ab26cc84cba1..b76f54dc8b0c5de7052552f202f62c9eefcce05a 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.h
@@ -26,20 +26,20 @@
#include <chrono>
#include <mutex>
-#include <boost/preprocessor/variadic/to_seq.hpp>
-#include <boost/preprocessor/seq/for_each.hpp>
#include <boost/current_function.hpp>
+#include <boost/preprocessor/seq/for_each.hpp>
+#include <boost/preprocessor/variadic/to_seq.hpp>
#include <VirtualRobot/Robot.h>
-#include <ArmarXCore/util/CPPUtility/Pointer.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/util/CPPUtility/Pointer.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include "../JointControllers/JointController.h"
-#include "../util/JointAndNJointControllers.h"
#include "../util.h"
+#include "../util/JointAndNJointControllers.h"
/**
* @defgroup Library-RobotUnit-Modules RobotUnitModules
@@ -51,23 +51,21 @@ namespace armarx
TYPEDEF_PTRS_HANDLE(RobotUnit);
TYPEDEF_PTRS_HANDLE(NJointControllerBase);
- template<class Targ, class Src>
- Targ& CheckedCastAndDeref(Src* src)
+ template <class Targ, class Src>
+ Targ&
+ CheckedCastAndDeref(Src* src)
{
Targ* ptr = dynamic_cast<Targ*>(src);
if (!ptr)
{
if (!src)
{
- throw std::invalid_argument {"Ptr passed to CheckedCastAndDeref is NULL"};
+ throw std::invalid_argument{"Ptr passed to CheckedCastAndDeref is NULL"};
}
- throw std::invalid_argument
- {
- "Ptr of type '" +
- GetTypeString<Src>() +
+ throw std::invalid_argument{
+ "Ptr of type '" + GetTypeString<Src>() +
"' passed to CheckedCastAndDeref is is not related to target type '" +
- GetTypeString<Targ>()
- };
+ GetTypeString<Targ>()};
}
return *ptr;
}
@@ -85,24 +83,29 @@ namespace armarx
};
std::string to_string(armarx::RobotUnitState s);
- inline std::ostream& operator<<(std::ostream& o, RobotUnitState s)
+
+ inline std::ostream&
+ operator<<(std::ostream& o, RobotUnitState s)
{
return o << to_string(s);
}
-}
+} // namespace armarx
namespace armarx::RobotUnitModule::detail
{
- class ModuleBaseIntermediatePropertyDefinitions: public ComponentPropertyDefinitions
+ class ModuleBaseIntermediatePropertyDefinitions : public ComponentPropertyDefinitions
{
public:
- ModuleBaseIntermediatePropertyDefinitions(): ComponentPropertyDefinitions("") {}
+ ModuleBaseIntermediatePropertyDefinitions() : ComponentPropertyDefinitions("")
+ {
+ }
};
-}
+} // namespace armarx::RobotUnitModule::detail
namespace armarx::RobotUnitModule
{
- class ModuleBasePropertyDefinitions: virtual public detail::ModuleBaseIntermediatePropertyDefinitions
+ class ModuleBasePropertyDefinitions :
+ virtual public detail::ModuleBaseIntermediatePropertyDefinitions
{
public:
ModuleBasePropertyDefinitions(std::string prefix)
@@ -112,9 +115,8 @@ namespace armarx::RobotUnitModule
}
};
-
#define forward_declare(r, data, Mod) TYPEDEF_PTRS_HANDLE(Mod);
- BOOST_PP_SEQ_FOR_EACH(forward_declare,, BOOST_PP_VARIADIC_TO_SEQ(RobotUnitModules))
+ BOOST_PP_SEQ_FOR_EACH(forward_declare, , BOOST_PP_VARIADIC_TO_SEQ(RobotUnitModules))
#undef forward_declare
/**
@@ -182,27 +184,33 @@ namespace armarx::RobotUnitModule
{
/// @brief The singleton instance
static std::atomic<ModuleBase*> Instance_;
+
public:
/**
* @brief Returns the singleton instance of this class
* @return The singleton instance of this class
*/
- static ModuleBase& Instance()
+ static ModuleBase&
+ Instance()
{
ARMARX_CHECK_NOT_NULL(Instance_);
return *Instance_;
}
+
/**
* @brief Returns the singleton instance of the given class
* @return The singleton instance of the given class
*/
- template<class T>
- static T& Instance()
+ template <class T>
+ static T&
+ Instance()
{
- static_assert(std::is_base_of<ModuleBase, T>::value, "The type has to derive ModuleBase");
+ static_assert(std::is_base_of<ModuleBase, T>::value,
+ "The type has to derive ModuleBase");
ARMARX_CHECK_NOT_NULL(Instance_);
return dynamic_cast<T&>(*Instance_);
}
+
protected:
/// @brief Ctor
ModuleBase();
@@ -220,8 +228,8 @@ namespace armarx::RobotUnitModule
using TimePointType = std::chrono::high_resolution_clock::time_point;
#define using_module_types(...) using_module_types_impl(__VA_ARGS__)
-#define using_module_types_impl(r, data, Mod) using Module ## Mod = Mod;
- BOOST_PP_SEQ_FOR_EACH(using_module_types,, BOOST_PP_VARIADIC_TO_SEQ(RobotUnitModules))
+#define using_module_types_impl(r, data, Mod) using Module##Mod = Mod;
+ BOOST_PP_SEQ_FOR_EACH(using_module_types, , BOOST_PP_VARIADIC_TO_SEQ(RobotUnitModules))
#undef using_module_types
#undef using_module_types_impl
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -230,39 +238,52 @@ namespace armarx::RobotUnitModule
protected:
/// @brief Checks whether the implementing class derives all modules.
void checkDerivedClasses() const;
+
public:
/**
* @brief Returns this as ref to the given type.
* @return This as ref to the given type.
*/
- template<class T> T& _module()
+ template <class T>
+ T&
+ _module()
{
- return dynamic_cast< T&>(*this);
+ return dynamic_cast<T&>(*this);
}
+
/**
* @brief Returns this as ref to the given type.
* @return This as ref to the given type.
*/
- template<class T> const T& _module() const
+ template <class T>
+ const T&
+ _module() const
{
return dynamic_cast<const T&>(*this);
}
+
/**
* @brief Returns this as ptr to the given type.
* @return This as ptr to the given type.
*/
- template<class T> T* _modulePtr()
+ template <class T>
+ T*
+ _modulePtr()
{
- return &_module< T>();
+ return &_module<T>();
}
+
/**
* @brief Returns this as ptr to the given type.
* @return This as ptr to the given type.
*/
- template<class T> const T* _modulePtr() const
+ template <class T>
+ const T*
+ _modulePtr() const
{
return &_module<const T>();
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ////////////////////////////// ManagedIceObject life cycle ///////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -272,12 +293,12 @@ namespace armarx::RobotUnitModule
* @brief \note This function is protected with \ref getGuard()
* @see \ref ManagedIceObject::onInitComponent
*/
- void onInitComponent() final override;
+ void onInitComponent() final override;
/**
* @brief \warning This function is not protected with \ref getGuard()
* @see \ref ManagedIceObject::onConnectComponent
*/
- void onConnectComponent() final override;
+ void onConnectComponent() final override;
/**
* @brief \warning This function is not protected with \ref getGuard()
* @see \ref ManagedIceObject::onDisconnectComponent
@@ -287,7 +308,7 @@ namespace armarx::RobotUnitModule
* @brief \note This function is protected with \ref getGuard()
* @see \ref ManagedIceObject::onExitComponent
*/
- void onExitComponent() final override;
+ void onExitComponent() final override;
//ManagedIceObject life cycle module hooks
/**
@@ -295,52 +316,81 @@ namespace armarx::RobotUnitModule
* \note This function is protected with \ref getGuard()
* @see onInitComponent
*/
- void _preOnInitRobotUnit() {}
+ void
+ _preOnInitRobotUnit()
+ {
+ }
+
/**
* @brief Hook for deriving RobotUnitModules called in \ref onInitComponent (called after \ref onInitRobotUnit was called)
* \note This function is protected with \ref getGuard()
* @see onInitComponent
*/
- void _postOnInitRobotUnit() {}
+ void
+ _postOnInitRobotUnit()
+ {
+ }
/**
* @brief Hook for deriving RobotUnitModules called in \ref onConnectComponent (called before \ref onConnectRobotUnit was called)
* \warning This function is not protected with \ref getGuard()
* @see onConnectComponent
*/
- void _preOnConnectRobotUnit() {}
+ void
+ _preOnConnectRobotUnit()
+ {
+ }
+
/**
* @brief Hook for deriving RobotUnitModules called in \ref onConnectComponent (called after \ref onConnectRobotUnit was called)
* \warning This function is not protected with \ref getGuard()
* @see onConnectComponent
*/
- void _postOnConnectRobotUnit() {}
+ void
+ _postOnConnectRobotUnit()
+ {
+ }
/**
* @brief Hook for deriving RobotUnitModules called in \ref onDisconnectComponent (called before \ref onDisconnectRobotUnit was called)
* \warning This function is not protected with \ref getGuard()
* @see onDisconnectComponent
*/
- void _preOnDisconnectRobotUnit() {}
+ void
+ _preOnDisconnectRobotUnit()
+ {
+ }
+
/**
* @brief Hook for deriving RobotUnitModules called in \ref onDisconnectComponent (called after \ref onDisconnectRobotUnit was called)
* \warning This function is not protected with \ref getGuard()
* @see onDisconnectComponent
*/
- void _postOnDisconnectRobotUnit() {}
+ void
+ _postOnDisconnectRobotUnit()
+ {
+ }
/**
* @brief Hook for deriving RobotUnitModules called in \ref onExitComponent (called before \ref onExitRobotUnit was called)
* \note This function is protected with \ref getGuard()
* @see onExitComponent
*/
- void _preOnExitRobotUnit() {}
+ void
+ _preOnExitRobotUnit()
+ {
+ }
+
/**
* @brief Hook for deriving RobotUnitModules called in \ref onExitComponent (called after \ref onExitRobotUnit was called)
* \note This function is protected with \ref getGuard()
* @see onExitComponent
*/
- void _postOnExitRobotUnit() {}
+ void
+ _postOnExitRobotUnit()
+ {
+ }
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ////////////////////// other ManagedIceObject / Component methods ////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -349,17 +399,27 @@ namespace armarx::RobotUnitModule
void componentPropertiesUpdated(const std::set<std::string>& changedProperties) override;
/// @see \ref ManagedIceObject::icePropertiesInitialized
void icePropertiesInitialized() override;
+
/// @see \ref ManagedIceObject::getDefaultName
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "RobotUnit";
}
//module hooks
/// @brief Hook for deriving RobotUnitModules called in \ref componentPropertiesUpdated
- void _componentPropertiesUpdated(const std::set<std::string>& /*changedProperties*/) {}
+ void
+ _componentPropertiesUpdated(const std::set<std::string>& /*changedProperties*/)
+ {
+ }
+
/// @brief Hook for deriving RobotUnitModules called in \ref icePropertiesInitialized
- void _icePropertiesInitialized() {}
+ void
+ _icePropertiesInitialized()
+ {
+ }
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ////////////////////////////// state and state transition ////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -369,6 +429,7 @@ namespace armarx::RobotUnitModule
* @param s The state to set
*/
void setRobotUnitState(RobotUnitState s);
+
protected:
//state transitions
/**
@@ -413,65 +474,101 @@ namespace armarx::RobotUnitModule
* \note This function is protected with \ref getGuard()
* @see finishComponentInitialization
*/
- void _preFinishComponentInitialization() {}
+ void
+ _preFinishComponentInitialization()
+ {
+ }
+
/**
* @brief Hook for deriving RobotUnitModules called in \ref finishComponentInitialization (called after the state has changed)
* \note This function is protected with \ref getGuard()
* @see finishComponentInitialization
*/
- void _postFinishComponentInitialization() {}
+ void
+ _postFinishComponentInitialization()
+ {
+ }
/**
* @brief Hook for deriving RobotUnitModules called in \ref finishDeviceInitialization (called before the state has changed)
* \note This function is protected with \ref getGuard()
* @see finishDeviceInitialization
*/
- void _preFinishDeviceInitialization() {}
+ void
+ _preFinishDeviceInitialization()
+ {
+ }
+
/**
* @brief Hook for deriving RobotUnitModules called in \ref finishDeviceInitialization (called after the state has changed)
* \note This function is protected with \ref getGuard()
* @see finishDeviceInitialization
*/
- void _postFinishDeviceInitialization() {}
+ void
+ _postFinishDeviceInitialization()
+ {
+ }
/**
* @brief Hook for deriving RobotUnitModules called in \ref finishUnitInitialization (called before the state has changed)
* \note This function is protected with \ref getGuard()
* @see finishUnitInitialization
*/
- void _preFinishUnitInitialization() {}
+ void
+ _preFinishUnitInitialization()
+ {
+ }
+
/**
* @brief Hook for deriving RobotUnitModules called in \ref finishUnitInitialization (called after the state has changed)
* \note This function is protected with \ref getGuard()
* @see finishUnitInitialization
*/
- void _postFinishUnitInitialization() {}
+ void
+ _postFinishUnitInitialization()
+ {
+ }
/**
* @brief Hook for deriving RobotUnitModules called in \ref finishControlThreadInitialization (called before the state has changed)
* \note This function is protected with \ref getGuard()
* @see finishControlThreadInitialization
*/
- void _preFinishControlThreadInitialization() {}
+ void
+ _preFinishControlThreadInitialization()
+ {
+ }
+
/**
* @brief Hook for deriving RobotUnitModules called in \ref finishControlThreadInitialization (called after the state has changed)
* \note This function is protected with \ref getGuard()
* @see finishControlThreadInitialization
*/
- void _postFinishControlThreadInitialization() {}
+ void
+ _postFinishControlThreadInitialization()
+ {
+ }
/**
* @brief Hook for deriving RobotUnitModules called in \ref finishRunning (called before the state has changed)
* \note This function is protected with \ref getGuard()
* @see finishRunning
*/
- void _preFinishRunning() {}
+ void
+ _preFinishRunning()
+ {
+ }
+
/**
* @brief Hook for deriving RobotUnitModules called in \ref finishRunning (called after the state has changed)
* \note This function is protected with \ref getGuard()
* @see finishRunning
*/
- void _postFinishRunning() {}
+ void
+ _postFinishRunning()
+ {
+ }
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// /////////////////////////////////////// utility //////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -488,21 +585,26 @@ namespace armarx::RobotUnitModule
* @param fnc The callers function name.
* @param onlyWarn Only print a warning instead of throwing an exception.
*/
- void throwIfStateIsNot(const std::set<RobotUnitState>& stateSet, const std::string& fnc, bool onlyWarn = false) const;
+ void throwIfStateIsNot(const std::set<RobotUnitState>& stateSet,
+ const std::string& fnc,
+ bool onlyWarn = false) const;
/**
* @brief Throws an exception if the current state is not \param state
* @param state The acceptable \ref RobotUnitState
* @param fnc The callers function name.
* @param onlyWarn Only print a warning instead of throwing an exception.
*/
- void throwIfStateIsNot(RobotUnitState s, const std::string& fnc, bool onlyWarn = false) const;
+ void
+ throwIfStateIsNot(RobotUnitState s, const std::string& fnc, bool onlyWarn = false) const;
/**
* @brief Throws an exception if the current state is in \param stateSet
* @param stateSet The set of forbidden \ref RobotUnitState "RobotUnitStates"
* @param fnc The callers function name.
* @param onlyWarn Only print a warning instead of throwing an exception.
*/
- void throwIfStateIs(const std::set<RobotUnitState>& stateSet, const std::string& fnc, bool onlyWarn = false) const;
+ void throwIfStateIs(const std::set<RobotUnitState>& stateSet,
+ const std::string& fnc,
+ bool onlyWarn = false) const;
/**
* @brief Throws an exception if the current state is \param state
* @param state The forbidden \ref RobotUnitState
@@ -541,18 +643,22 @@ namespace armarx::RobotUnitModule
* @return Whether the \ref RobotUnit is shutting down
* @see shutDown
*/
- bool isShuttingDown() const ///TODO use this function in implementations
+ bool
+ isShuttingDown() const ///TODO use this function in implementations
{
return isShuttingDown_.load();
}
+
/**
* @brief Requests the \ref RobotUnit to shut down.
* @see isShuttingDown
*/
- void shutDown() ///TODO use this function in implementations
+ void
+ shutDown() ///TODO use this function in implementations
{
isShuttingDown_.store(true);
}
+
// //////////////////////////////////////////////////////////////////////////// //
// /////////////////////////// robot unit impl hooks ////////////////////////// //
// //////////////////////////////////////////////////////////////////////////// //
@@ -561,26 +667,41 @@ namespace armarx::RobotUnitModule
* \note This function is protected with \ref getGuard()
* @see onInitComponent
*/
- virtual void onInitRobotUnit() {}
+ virtual void
+ onInitRobotUnit()
+ {
+ }
+
/**
* @brief called in onConnectComponent
* \warning This function is not protected with \ref getGuard()
* @see onConnectComponent
*/
- virtual void onConnectRobotUnit() {}
+ virtual void
+ onConnectRobotUnit()
+ {
+ }
+
/**
* @brief called in onDisconnecComponent
* \warning This function is not protected with \ref getGuard()
* @see onDisconnecComponent
*/
- virtual void onDisconnectRobotUnit() {}
+ virtual void
+ onDisconnectRobotUnit()
+ {
+ }
+
/**
* @brief called in onExitComponent before calling finishRunning
* \note This function is protected with \ref getGuard()
* @see finishRunning
* @see onExitComponent
*/
- virtual void onExitRobotUnit() {}
+ virtual void
+ onExitRobotUnit()
+ {
+ }
/// @brief Implementations have to join their \ref ControlThread in this hook. (used by RobotUnit::finishRunning())
virtual void joinControlThread() = 0;
@@ -595,10 +716,12 @@ namespace armarx::RobotUnitModule
* @brief Returns a sentinel value for an index (std::numeric_limits<std::size_t>::max())
* @return A sentinel value for an index (std::numeric_limits<std::size_t>::max())
*/
- static constexpr std::size_t IndexSentinel()
+ static constexpr std::size_t
+ IndexSentinel()
{
return std::numeric_limits<std::size_t>::max();
}
+
/**
* @brief Returns a guard to the \ref RobotUnits mutex.
* @return A guard to the \ref RobotUnits mutex.
@@ -614,11 +737,10 @@ namespace armarx::RobotUnitModule
/// @brief Set of \ref RobotUnitState "RobotUnitStates" where \ref Device "Devices" are usable
static const std::set<RobotUnitState> DevicesReadyStates;
/// @brief The \ref RobotUnit's current state
- std::atomic<RobotUnitState> state {RobotUnitState::InitializingComponent};
+ std::atomic<RobotUnitState> state{RobotUnitState::InitializingComponent};
/// @brief Whether the \ref RobotUnit is shutting down
- std::atomic_bool isShuttingDown_ {false};
+ std::atomic_bool isShuttingDown_{false};
};
-}
+} // namespace armarx::RobotUnitModule
#include "RobotUnitModuleBase.ipp"
-
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
index 5d7d7d01ece91cb274e2f4e6a6f0619c06f4a65e..e0131a416855174e5571d22d9e44cfac50f76954 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
@@ -29,16 +29,17 @@
#include <ArmarXCore/core/util/OnScopeExit.h>
#include <ArmarXCore/observers/filters/rtfilters/AverageFilter.h>
#include <ArmarXCore/observers/filters/rtfilters/ButterworthFilter.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h>
+
#include <RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h>
-#include <RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h>
+#include <RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h>
#include "../Devices/RTThreadTimingsSensorDevice.h"
@@ -51,22 +52,26 @@ namespace armarx::RobotUnitModule
class NJointControllerAttorneyForControlThread
{
friend class ControlThread;
+
static void
RtDeactivateController(const NJointControllerBasePtr& nJointCtrl)
{
nJointCtrl->rtDeactivateController();
}
+
static void
RtActivateController(const NJointControllerBasePtr& nJointCtrl)
{
nJointCtrl->rtActivateController();
}
+
static void
RtDeactivateControllerBecauseOfError(const NJointControllerBasePtr& nJointCtrl)
{
nJointCtrl->rtDeactivateControllerBecauseOfError();
}
};
+
/**
* \brief This class allows minimal access to private members of \ref ControlThreadDataBuffer in a sane fashion for \ref ControlThread.
* \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
@@ -74,6 +79,7 @@ namespace armarx::RobotUnitModule
class ControlThreadDataBufferAttorneyForControlThread
{
friend class ControlThread;
+
static std::vector<JointController*>&
GetActivatedJointControllers(ControlThread* p)
{
@@ -81,6 +87,7 @@ namespace armarx::RobotUnitModule
.controllersActivated.getWriteBuffer()
.jointControllers;
}
+
static std::vector<NJointControllerBase*>&
GetActivatedNJointControllers(ControlThread* p)
{
@@ -88,6 +95,7 @@ namespace armarx::RobotUnitModule
.controllersActivated.getWriteBuffer()
.nJointControllers;
}
+
static std::vector<std::size_t>&
GetActivatedJointToNJointControllerAssignement(ControlThread* p)
{
@@ -95,6 +103,7 @@ namespace armarx::RobotUnitModule
.controllersActivated.getWriteBuffer()
.jointToNJointControllerAssignement;
}
+
static const std::vector<JointController*>&
GetActivatedJointControllers(const ControlThread* p)
{
@@ -102,6 +111,7 @@ namespace armarx::RobotUnitModule
.controllersActivated.getWriteBuffer()
.jointControllers;
}
+
static const std::vector<NJointControllerBase*>&
GetActivatedNJointControllers(const ControlThread* p)
{
@@ -109,6 +119,7 @@ namespace armarx::RobotUnitModule
.controllersActivated.getWriteBuffer()
.nJointControllers;
}
+
static const std::vector<std::size_t>&
GetActivatedJointToNJointControllerAssignement(const ControlThread* p)
{
@@ -126,11 +137,13 @@ namespace armarx::RobotUnitModule
.controllersRequested.getReadBuffer()
.jointToNJointControllerAssignement;
}
+
static void
CommitActivatedControllers(ControlThread* p)
{
return p->_module<ControlThreadDataBuffer>().controllersActivated.commitWrite();
}
+
static void
ResetActivatedControllerAssignement(ControlThread* p)
{
@@ -147,6 +160,7 @@ namespace armarx::RobotUnitModule
.controllersRequested.getReadBuffer()
.jointControllers;
}
+
static const std::vector<NJointControllerBase*>&
GetRequestedNJointControllers(const ControlThread* p)
{
@@ -155,6 +169,7 @@ namespace armarx::RobotUnitModule
.controllersRequested.getReadBuffer()
.nJointControllers;
}
+
static const std::vector<std::size_t>&
GetRequestedJointToNJointControllerAssignement(const ControlThread* p)
{
@@ -199,6 +214,7 @@ namespace armarx::RobotUnitModule
j.at(index) = c;
}
};
+
/**
* \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref ControlThread.
* \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
@@ -206,11 +222,13 @@ namespace armarx::RobotUnitModule
class DevicesAttorneyForControlThread
{
friend class ControlThread;
+
static const std::vector<ControlDevicePtr>&
GetControlDevices(const ControlThread* p)
{
return p->_module<Devices>().controlDevices.values();
}
+
static const std::vector<SensorDevicePtr>&
GetSensorDevices(const ControlThread* p)
{
@@ -222,11 +240,13 @@ namespace armarx::RobotUnitModule
{
return p->_module<Devices>().sensorValues;
}
+
static const std::vector<std::vector<PropagateConst<ControlTargetBase*>>>&
GetControlTargets(const ControlThread* p)
{
return p->_module<Devices>().controlTargets;
}
+
static RTThreadTimingsSensorDevice&
GetThreadTimingsSensorDevice(const ControlThread* p)
{
@@ -242,6 +262,7 @@ namespace armarx::RobotUnitModule
robot, robotNodes, p->_module<Devices>().sensorValues);
}
};
+
/**
* \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref ControlThread.
* \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
@@ -249,6 +270,7 @@ namespace armarx::RobotUnitModule
class ManagementAttorneyForControlThread
{
friend class ControlThread;
+
static bool
HeartbeatMissing(const ControlThread* p)
{
@@ -1099,7 +1121,6 @@ namespace armarx::RobotUnitModule
}
}
-
void
ControlThread::dumpRtControllerSetup(
std::ostream& out,
@@ -1179,14 +1200,16 @@ namespace armarx::RobotUnitModule
void
ControlThread::rtUpdateRobotGlobalPose()
{
- const ConstSensorDevicePtr globalPoseSensorDevice = _module<Devices>().getSensorDevice(GlobalRobotPoseSensorDevice::DeviceName());
- if(not globalPoseSensorDevice)
+ const ConstSensorDevicePtr globalPoseSensorDevice =
+ _module<Devices>().getSensorDevice(GlobalRobotPoseSensorDevice::DeviceName());
+ if (not globalPoseSensorDevice)
{
return;
}
- const auto *const sensorValue = globalPoseSensorDevice->getSensorValue()->asA<SensorValueGlobalRobotPose>();
- if(sensorValue == nullptr)
+ const auto* const sensorValue =
+ globalPoseSensorDevice->getSensorValue()->asA<SensorValueGlobalRobotPose>();
+ if (sensorValue == nullptr)
{
return;
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
index 9b14a13ba370166793e188c8c8577fcc5ea19516..af1990164ed130b335b6e8c26b540d165694b51e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
@@ -28,44 +28,47 @@
#include "../Devices/ControlDevice.h"
#include "../Devices/SensorDevice.h"
-
#include "RobotUnitModuleBase.h"
namespace armarx
{
class DynamicsHelper;
class RTThreadTimingsSensorDevice;
-}
+} // namespace armarx
namespace armarx::RobotUnitModule
{
- class ControlThreadPropertyDefinitions: public ModuleBasePropertyDefinitions
+ class ControlThreadPropertyDefinitions : public ModuleBasePropertyDefinitions
{
public:
- ControlThreadPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
+ ControlThreadPropertyDefinitions(std::string prefix) : ModuleBasePropertyDefinitions(prefix)
{
defineOptionalProperty<std::size_t>(
- "NjointController_AllowedExecutionTimePerControlDeviceUntilWarning", 2,
- "A Warning will be printed, If the execution time in micro seconds of a NJointControllerBase "
- "exceeds this parameter times the number of ControlDevices."
- );
+ "NjointController_AllowedExecutionTimePerControlDeviceUntilWarning",
+ 2,
+ "A Warning will be printed, If the execution time in micro seconds of a "
+ "NJointControllerBase "
+ "exceeds this parameter times the number of ControlDevices.");
defineOptionalProperty<std::size_t>(
- "NjointController_AllowedExecutionTimePerControlDeviceUntilError", 20,
- "An Error will be printed, If the execution time in micro seconds of a NJointControllerBase "
- "exceeds this parameter times the number of ControlDevices."
- );
- defineOptionalProperty<bool>("EnableInverseDynamics", false,
- "If true, inverse dynamics are calculated for all joints given in the "
- "InverseDynamicsRobotJointSets property during each loop of "
- "the rt control thread.");
- defineOptionalProperty<std::string>(
- "InverseDynamicsRobotJointSets", "LeftArm,RightArm",
- "Comma separated list of robot nodesets for which the inverse dynamics should be calculcated."
- );
+ "NjointController_AllowedExecutionTimePerControlDeviceUntilError",
+ 20,
+ "An Error will be printed, If the execution time in micro seconds of a "
+ "NJointControllerBase "
+ "exceeds this parameter times the number of ControlDevices.");
+ defineOptionalProperty<bool>(
+ "EnableInverseDynamics",
+ false,
+ "If true, inverse dynamics are calculated for all joints given in the "
+ "InverseDynamicsRobotJointSets property during each loop of "
+ "the rt control thread.");
+ defineOptionalProperty<std::string>("InverseDynamicsRobotJointSets",
+ "LeftArm,RightArm",
+ "Comma separated list of robot nodesets for which "
+ "the inverse dynamics should be calculcated.");
defineOptionalProperty<std::string>(
- "InverseDynamicsRobotBodySet", "RobotCol",
- "Robot nodeset of which the masses should be used for the inverse dynamics"
- );
+ "InverseDynamicsRobotBodySet",
+ "RobotCol",
+ "Robot nodeset of which the masses should be used for the inverse dynamics");
}
};
@@ -86,6 +89,7 @@ namespace armarx::RobotUnitModule
RequestInactive,
RequestNone,
};
+
protected:
enum class SwitchControllerMode
{
@@ -96,15 +100,18 @@ namespace armarx::RobotUnitModule
RTThread,
IceRequests
};
+
public:
/**
* @brief Returns the singleton instance of this class
* @return The singleton instance of this class
*/
- static ControlThread& Instance()
+ static ControlThread&
+ Instance()
{
return ModuleBase::Instance<ControlThread>();
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -121,17 +128,20 @@ namespace armarx::RobotUnitModule
* @brief Sets the \ref EmergencyStopState
* @param state The \ref EmergencyStopState to set
*/
- void setEmergencyStopState(EmergencyStopState state, const Ice::Current& = Ice::emptyCurrent) override;
+ void setEmergencyStopState(EmergencyStopState state,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Returns the \ref ControlThread's target \ref EmergencyStopState
* @return The \ref EmergencyStopState
*/
- EmergencyStopState getEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const override;
+ EmergencyStopState
+ getEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the \ref ControlThread's \ref EmergencyStopState
* @return The \ref EmergencyStopState
*/
- EmergencyStopState getRtEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const override;
+ EmergencyStopState
+ getRtEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const override;
// //////////////////////////////////////////////////////////////////////////////////////// //
// /////////////////////////////////// Module interface /////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -141,10 +151,12 @@ namespace armarx::RobotUnitModule
* @return The \ref ControlThread's thread id
* @see controlThreadId
*/
- std::thread::id getControlThreadId() const
+ std::thread::id
+ getControlThreadId() const
{
return controlThreadId;
}
+
private:
/**
* @brief Sets the \ref EmergencyStopState without updating the topic.
@@ -166,10 +178,12 @@ namespace armarx::RobotUnitModule
* @brief Returns the simox robot used in the control thread
* @return The simox robot used in the control thread
*/
- const VirtualRobot::RobotPtr& rtGetRobot() const
+ const VirtualRobot::RobotPtr&
+ rtGetRobot() const
{
return rtRobot;
}
+
/**
* @brief Changes the set of active controllers.
* Changes can be caused by a new set of requested controllers or emergency stop
@@ -194,29 +208,34 @@ namespace armarx::RobotUnitModule
* @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
* @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
*/
- void rtRunNJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ void rtRunNJointControllers(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration);
/**
* @brief Runs Joint controllers and writes target values to ControlDevices
* @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
* @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
*/
- void rtRunJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ void rtRunJointControllers(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration);
/**
* @brief Calls \ref SensorDevice::rtReadSensorValues for all \ref SensorDevice "SensorDevices".
* @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
* @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
*/
- void rtReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ void rtReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration);
/**
* @brief Execute code after updating the sensor values. Default implementation calculates the inverse dynamics.
* @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
* @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
*/
- void virtual rtPostReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ void virtual rtPostReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration);
/// @brief Deactivates all NJointControllers generating invalid targets.
void rtHandleInvalidTargets();
/// @brief Calls \ref JointController::rtResetTarget for all active Joint controllers
void rtResetAllTargets();
+
/**
* @brief Hook for deriving classes (called AFTER a NJointControllerBase is deactivated due to an error)
*
@@ -226,14 +245,19 @@ namespace armarx::RobotUnitModule
* Since this hook is called after the controller with an error was deactivated an other call to
* \ref rtDeactivateAssignedNJointControllerBecauseOfError can't lead to a cyclic execution.
*/
- virtual void rtDeactivatedNJointControllerBecauseOfError(const NJointControllerBasePtr& /*nJointCtrl*/) {}
+ virtual void
+ rtDeactivatedNJointControllerBecauseOfError(const NJointControllerBasePtr& /*nJointCtrl*/)
+ {
+ }
+
/**
* @brief Updates the current \ref SensorValueBase "SensorValues" and \ref ControlTargetBase "ControlTargets"
* for code running outside of the \ref ControlThread
* @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
* @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
*/
- void rtUpdateSensorAndControlBuffer(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ void rtUpdateSensorAndControlBuffer(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration);
/**
* @brief Returns all \ref ControlDevice "ControlDevices"
@@ -255,17 +279,21 @@ namespace armarx::RobotUnitModule
* @brief Returns whether the rt thread is in emergency stop. (This function is for use in the \ref ControlThread)
* @return Whether the rt thread is in emergency stop.
*/
- bool rtIsInEmergencyStop() const
+ bool
+ rtIsInEmergencyStop() const
{
return rtIsInEmergencyStop_;
}
+
/**
* @brief Returns the \ref ControlThread's emergency stop state. (This function is for use in the \ref ControlThread)
* @return The \ref ControlThread's emergency stop state
*/
- EmergencyStopState rtGetEmergencyStopState() const
+ EmergencyStopState
+ rtGetEmergencyStopState() const
{
- return rtIsInEmergencyStop_ ? EmergencyStopState::eEmergencyStopActive : EmergencyStopState::eEmergencyStopInactive;
+ return rtIsInEmergencyStop_ ? EmergencyStopState::eEmergencyStopActive
+ : EmergencyStopState::eEmergencyStopInactive;
}
/**
@@ -327,21 +355,20 @@ namespace armarx::RobotUnitModule
void rtSyncNJointControllerRobot(NJointControllerBase* njctrl);
- void dumpRtControllerSetup(
- std::ostream& out,
- const std::string& indent,
- const std::vector<JointController*>& activeCdevs,
- const std::vector<std::size_t>& activeJCtrls,
- const std::vector<NJointControllerBase*>& assignement) const;
+ void dumpRtControllerSetup(std::ostream& out,
+ const std::string& indent,
+ const std::vector<JointController*>& activeCdevs,
+ const std::vector<std::size_t>& activeJCtrls,
+ const std::vector<NJointControllerBase*>& assignement) const;
std::string dumpRtState() const;
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////////////// Data ///////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
private:
///@brief Whether the ControlThread is in emergency stop
- std::atomic_bool rtIsInEmergencyStop_ {false};
+ std::atomic_bool rtIsInEmergencyStop_{false};
///@brief Whether the ControlThread is supposed to be in emergency stop
- std::atomic_bool emergencyStop {false};
+ std::atomic_bool emergencyStop{false};
///@brief The \ref VirtualRobot::Robot used in the \ref ControlThread
VirtualRobot::RobotPtr rtRobot;
@@ -356,8 +383,8 @@ namespace armarx::RobotUnitModule
/// @brief An Error will be printed, if the execution time per ControlDev of a NJointControllerBase exceeds this parameter
std::size_t usPerDevUntilError;
- bool rtSwitchControllerSetupChangedControllers {false};
- std::size_t numberOfInvalidTargetsInThisIteration {0};
+ bool rtSwitchControllerSetupChangedControllers{false};
+ std::size_t numberOfInvalidTargetsInThisIteration{0};
std::vector<JointController*> preSwitchSetup_ActivatedJointControllers;
std::vector<std::size_t> preSwitchSetup_ActivatedJointToNJointControllerAssignement;
@@ -370,7 +397,8 @@ namespace armarx::RobotUnitModule
std::vector<std::size_t> _activatedSynchronousNJointControllersIdx;
std::vector<std::size_t> _activatedAsynchronousNJointControllersIdx;
- std::atomic<EmergencyStopStateRequest> emergencyStopStateRequest {EmergencyStopStateRequest::RequestNone};
+ std::atomic<EmergencyStopStateRequest> emergencyStopStateRequest{
+ EmergencyStopStateRequest::RequestNone};
std::shared_ptr<DynamicsHelper> dynamicsHelpers;
@@ -390,4 +418,4 @@ namespace armarx::RobotUnitModule
*/
friend class ControlThreadAttorneyForRobotUnitEmergencyStopMaster;
};
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp
index 891c8fbf11edd151630ebb3e6fa4d244b6bc71ea..99b4a64e42605d0449e62145b869d8d64972aba7 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp
@@ -22,11 +22,11 @@
#include "RobotUnitModuleControlThreadDataBuffer.h"
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
+
#include "RobotUnitModuleControllerManagement.h"
#include "RobotUnitModuleDevices.h"
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
-
namespace armarx::RobotUnitModule
{
/**
@@ -36,11 +36,14 @@ namespace armarx::RobotUnitModule
class DevicesAttorneyForControlThreadDataBuffer
{
friend class ControlThreadDataBuffer;
- static std::vector<JointController*> GetStopMovementJointControllers(ControlThreadDataBuffer* p)
+
+ static std::vector<JointController*>
+ GetStopMovementJointControllers(ControlThreadDataBuffer* p)
{
return p->_module<Devices>().getStopMovementJointControllers();
}
};
+
/**
* \brief This class allows minimal access to private members of \ref ControllerManagement in a sane fashion for \ref ControlThreadDataBuffer.
* \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
@@ -49,121 +52,134 @@ namespace armarx::RobotUnitModule
{
friend class ControlThreadDataBuffer;
- static void UpdateNJointControllerRequestedState(ControlThreadDataBuffer* p, const std::set<NJointControllerBasePtr>& request)
+ static void
+ UpdateNJointControllerRequestedState(ControlThreadDataBuffer* p,
+ const std::set<NJointControllerBasePtr>& request)
{
p->_module<ControllerManagement>().updateNJointControllerRequestedState(request);
}
};
-}
+} // namespace armarx::RobotUnitModule
namespace armarx::RobotUnitModule
{
- JointAndNJointControllers ControlThreadDataBuffer::getActivatedControllers() const
+ JointAndNJointControllers
+ ControlThreadDataBuffer::getActivatedControllers() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<std::recursive_mutex> guard {controllersActivatedMutex};
+ std::lock_guard<std::recursive_mutex> guard{controllersActivatedMutex};
return controllersActivated.getReadBuffer();
}
- std::vector<JointController*> ControlThreadDataBuffer::getActivatedJointControllers() const
+ std::vector<JointController*>
+ ControlThreadDataBuffer::getActivatedJointControllers() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<std::recursive_mutex> guard {controllersActivatedMutex};
+ std::lock_guard<std::recursive_mutex> guard{controllersActivatedMutex};
return controllersActivated.getReadBuffer().jointControllers;
}
- std::vector<NJointControllerBase*> ControlThreadDataBuffer::getActivatedNJointControllers() const
+ std::vector<NJointControllerBase*>
+ ControlThreadDataBuffer::getActivatedNJointControllers() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<std::recursive_mutex> guard {controllersActivatedMutex};
+ std::lock_guard<std::recursive_mutex> guard{controllersActivatedMutex};
return controllersActivated.getReadBuffer().nJointControllers;
}
- bool ControlThreadDataBuffer::activatedControllersChanged() const
+
+ bool
+ ControlThreadDataBuffer::activatedControllersChanged() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<std::recursive_mutex> guard {controllersActivatedMutex};
+ std::lock_guard<std::recursive_mutex> guard{controllersActivatedMutex};
return controllersActivated.updateReadBuffer();
}
- JointAndNJointControllers ControlThreadDataBuffer::copyRequestedControllers() const
+ JointAndNJointControllers
+ ControlThreadDataBuffer::copyRequestedControllers() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<std::recursive_mutex> guard {controllersRequestedMutex};
+ std::lock_guard<std::recursive_mutex> guard{controllersRequestedMutex};
return controllersRequested.getWriteBuffer();
}
- std::vector<JointController*> ControlThreadDataBuffer::copyRequestedJointControllers() const
+ std::vector<JointController*>
+ ControlThreadDataBuffer::copyRequestedJointControllers() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<std::recursive_mutex> guard {controllersRequestedMutex};
+ std::lock_guard<std::recursive_mutex> guard{controllersRequestedMutex};
return controllersRequested.getWriteBuffer().jointControllers;
}
- std::vector<NJointControllerBase*> ControlThreadDataBuffer::copyRequestedNJointControllers() const
+ std::vector<NJointControllerBase*>
+ ControlThreadDataBuffer::copyRequestedNJointControllers() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<std::recursive_mutex> guard {controllersRequestedMutex};
+ std::lock_guard<std::recursive_mutex> guard{controllersRequestedMutex};
return controllersRequested.getWriteBuffer().nJointControllers;
}
- bool ControlThreadDataBuffer::sensorAndControlBufferChanged() const
+ bool
+ ControlThreadDataBuffer::sensorAndControlBufferChanged() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
return controlThreadOutputBuffer.updateReadBuffer();
}
- const SensorAndControl& ControlThreadDataBuffer::getSensorAndControlBuffer() const
+ const SensorAndControl&
+ ControlThreadDataBuffer::getSensorAndControlBuffer() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
return controlThreadOutputBuffer.getReadBuffer();
}
- void ControlThreadDataBuffer::writeRequestedControllers(JointAndNJointControllers&& setOfControllers)
+ void
+ ControlThreadDataBuffer::writeRequestedControllers(JointAndNJointControllers&& setOfControllers)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
//check NJoint
const auto& nJointCtrls = setOfControllers.nJointControllers;
- std::set<NJointControllerBasePtr> nJointSet {nJointCtrls.begin(), nJointCtrls.end()};
+ std::set<NJointControllerBasePtr> nJointSet{nJointCtrls.begin(), nJointCtrls.end()};
nJointSet.erase(nullptr);
//# NJoint
- const std::size_t nNJointCtrls = std::count_if(nJointCtrls.begin(), nJointCtrls.end(), [](const NJointControllerBasePtr & p)
- {
- return p;
- });
+ const std::size_t nNJointCtrls =
+ std::count_if(nJointCtrls.begin(),
+ nJointCtrls.end(),
+ [](const NJointControllerBasePtr& p) { return p; });
ARMARX_CHECK_EXPRESSION(nNJointCtrls == nJointSet.size());
- ARMARX_CHECK_EXPRESSION(nJointCtrls.size() == _module<Devices>().getNumberOfControlDevices());
+ ARMARX_CHECK_EXPRESSION(nJointCtrls.size() ==
+ _module<Devices>().getNumberOfControlDevices());
//first nNJointCtrls not null
- ARMARX_CHECK_EXPRESSION(std::all_of(nJointCtrls.begin(), nJointCtrls.begin() + nNJointCtrls, [](NJointControllerBase * p)
- {
- return p;
- }));
+ ARMARX_CHECK_EXPRESSION(std::all_of(nJointCtrls.begin(),
+ nJointCtrls.begin() + nNJointCtrls,
+ [](NJointControllerBase* p) { return p; }));
//last getNumberOfControlDevices()-nNJointCtrls null
- ARMARX_CHECK_EXPRESSION(std::all_of(nJointCtrls.begin() + nNJointCtrls, nJointCtrls.end(), [](NJointControllerBase * p)
- {
- return !p;
- }));
+ ARMARX_CHECK_EXPRESSION(std::all_of(nJointCtrls.begin() + nNJointCtrls,
+ nJointCtrls.end(),
+ [](NJointControllerBase* p) { return !p; }));
//conflict free and sorted
- ARMARX_CHECK_EXPRESSION(std::is_sorted(nJointCtrls.begin(), nJointCtrls.end(), std::greater<NJointControllerBase*> {}));
- ARMARX_CHECK_EXPRESSION(NJointControllerBase::AreNotInConflict(nJointCtrls.begin(), nJointCtrls.begin() + nNJointCtrls));
+ ARMARX_CHECK_EXPRESSION(std::is_sorted(
+ nJointCtrls.begin(), nJointCtrls.end(), std::greater<NJointControllerBase*>{}));
+ ARMARX_CHECK_EXPRESSION(NJointControllerBase::AreNotInConflict(
+ nJointCtrls.begin(), nJointCtrls.begin() + nNJointCtrls));
//check JointCtrl
const auto& jointCtrls = setOfControllers.jointControllers;
- ARMARX_CHECK_EXPRESSION(jointCtrls.size() == _module<Devices>().getNumberOfControlDevices());
- ARMARX_CHECK_EXPRESSION(std::all_of(jointCtrls.begin(), jointCtrls.end(), [](JointController * p)
- {
- return p;
- }));
+ ARMARX_CHECK_EXPRESSION(jointCtrls.size() ==
+ _module<Devices>().getNumberOfControlDevices());
+ ARMARX_CHECK_EXPRESSION(std::all_of(
+ jointCtrls.begin(), jointCtrls.end(), [](JointController* p) { return p; }));
//check groups
{
@@ -172,13 +188,17 @@ namespace armarx::RobotUnitModule
for (const auto& group : _module<Devices>().getControlModeGroups().groups)
{
ARMARX_CHECK_EXPRESSION(!group.empty());
- const auto hwMode = setOfControllers.jointControllers.at(_module<Devices>().getControlDeviceIndex(*group.begin()))->getHardwareControlMode();
+ const auto hwMode =
+ setOfControllers.jointControllers
+ .at(_module<Devices>().getControlDeviceIndex(*group.begin()))
+ ->getHardwareControlMode();
ARMARX_DEBUG << "---- group " << grpIdx << " mode '" << hwMode << "'";
for (const auto& other : group)
{
const auto& dev = _module<Devices>().getControlDeviceIndex(other);
ARMARX_CHECK_EXPRESSION(dev);
- const auto otherHwMode = setOfControllers.jointControllers.at(dev)->getHardwareControlMode();
+ const auto otherHwMode =
+ setOfControllers.jointControllers.at(dev)->getHardwareControlMode();
ARMARX_DEBUG << "-------- '" << other << "'' mode '" << otherHwMode << "'";
ARMARX_CHECK_EXPRESSION(otherHwMode == hwMode);
}
@@ -189,9 +209,12 @@ namespace armarx::RobotUnitModule
{
ARMARX_DEBUG << "setup assignement index";
setOfControllers.resetAssignement();
- for (std::size_t nJointCtrlIndex = 0; nJointCtrlIndex < setOfControllers.nJointControllers.size(); ++nJointCtrlIndex)
+ for (std::size_t nJointCtrlIndex = 0;
+ nJointCtrlIndex < setOfControllers.nJointControllers.size();
+ ++nJointCtrlIndex)
{
- const NJointControllerBase* nJoint = setOfControllers.nJointControllers.at(nJointCtrlIndex);
+ const NJointControllerBase* nJoint =
+ setOfControllers.nJointControllers.at(nJointCtrlIndex);
if (!nJoint)
{
break;
@@ -199,9 +222,14 @@ namespace armarx::RobotUnitModule
ARMARX_DEBUG << "---- " << nJoint->getInstanceName();
for (std::size_t jointIndex : nJoint->getControlDeviceUsedIndices())
{
- ARMARX_DEBUG << "-------- Index " << jointIndex << ": " << setOfControllers.jointToNJointControllerAssignement.at(jointIndex) << "-> " << nJointCtrlIndex;
- ARMARX_CHECK_EXPRESSION(setOfControllers.jointToNJointControllerAssignement.at(jointIndex) == IndexSentinel());
- setOfControllers.jointToNJointControllerAssignement.at(jointIndex) = nJointCtrlIndex;
+ ARMARX_DEBUG << "-------- Index " << jointIndex << ": "
+ << setOfControllers.jointToNJointControllerAssignement.at(
+ jointIndex)
+ << "-> " << nJointCtrlIndex;
+ ARMARX_CHECK_EXPRESSION(setOfControllers.jointToNJointControllerAssignement.at(
+ jointIndex) == IndexSentinel());
+ setOfControllers.jointToNJointControllerAssignement.at(jointIndex) =
+ nJointCtrlIndex;
}
}
}
@@ -211,40 +239,45 @@ namespace armarx::RobotUnitModule
checkSetOfControllersToActivate(setOfControllers);
}
//set requested state
- ControllerManagementAttorneyForControlThreadDataBuffer::UpdateNJointControllerRequestedState(this, nJointSet);
+ ControllerManagementAttorneyForControlThreadDataBuffer::
+ UpdateNJointControllerRequestedState(this, nJointSet);
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), setOfControllers.jointControllers.size());
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), setOfControllers.nJointControllers.size());
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), setOfControllers.jointToNJointControllerAssignement.size());
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
+ setOfControllers.jointControllers.size());
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
+ setOfControllers.nJointControllers.size());
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
+ setOfControllers.jointToNJointControllerAssignement.size());
{
- std::lock_guard<std::recursive_mutex> guard {controllersRequestedMutex};
+ std::lock_guard<std::recursive_mutex> guard{controllersRequestedMutex};
controllersRequested.getWriteBuffer() = std::move(setOfControllers);
controllersRequested.commitWrite();
}
- ARMARX_DEBUG << "writeRequestedControllers(JointAndNJointControllers&& setOfControllers)...done!";
+ ARMARX_DEBUG
+ << "writeRequestedControllers(JointAndNJointControllers&& setOfControllers)...done!";
}
- void ControlThreadDataBuffer::setActivateControllersRequest(std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> ctrls)
+ void
+ ControlThreadDataBuffer::setActivateControllersRequest(
+ std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> ctrls)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::recursive_mutex> guardRequested {controllersRequestedMutex};
+ std::lock_guard<std::recursive_mutex> guardRequested{controllersRequestedMutex};
throwIfDevicesNotReady(__FUNCTION__);
//erase nullptr
ctrls.erase(nullptr);
ARMARX_CHECK_EXPRESSION(ctrls.size() <= _module<Devices>().getNumberOfControlDevices());
- const std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> setOfRequestedCtrls
- {
- controllersRequested.getWriteBuffer().nJointControllers.begin(),
- controllersRequested.getWriteBuffer().nJointControllers.end()
- };
+ const std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>>
+ setOfRequestedCtrls{controllersRequested.getWriteBuffer().nJointControllers.begin(),
+ controllersRequested.getWriteBuffer().nJointControllers.end()};
if (setOfRequestedCtrls == ctrls)
{
//redirty the flag to swap in the same set again
controllersRequested.commitWrite();
return;
}
- JointAndNJointControllers request {_module<Devices>().getNumberOfControlDevices()};
+ JointAndNJointControllers request{_module<Devices>().getNumberOfControlDevices()};
std::size_t idx = 0;
for (const NJointControllerBasePtr& nJointCtrl : ctrls)
{
@@ -252,19 +285,25 @@ namespace armarx::RobotUnitModule
//add Joint for this ctrl
for (const auto& cd2cm : nJointCtrl->getControlDeviceUsedControlModeMap())
{
- std::size_t jointCtrlIndex = _module<Devices>().getControlDevices().index(cd2cm.first);
+ std::size_t jointCtrlIndex =
+ _module<Devices>().getControlDevices().index(cd2cm.first);
if (request.jointControllers.at(jointCtrlIndex))
{
std::stringstream ss;
- ss << "RobotUnit:setActivateControllersRequest controllers to activate are in conflict!\ncontrollers:";
+ 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()};
+ throw InvalidArgumentException{ss.str()};
}
- request.jointControllers.at(jointCtrlIndex) = _module<Devices>().getControlDevices().at(jointCtrlIndex)->getJointController(cd2cm.second);
+ request.jointControllers.at(jointCtrlIndex) =
+ _module<Devices>()
+ .getControlDevices()
+ .at(jointCtrlIndex)
+ ->getJointController(cd2cm.second);
}
}
@@ -274,8 +313,10 @@ namespace armarx::RobotUnitModule
JointController*& jointCtrl = request.jointControllers.at(i);
if (!jointCtrl)
{
- JointController* jointCtrlOld = controllersRequested.getWriteBuffer().jointControllers.at(i);
- if (jointCtrlOld == _module<Devices>().getControlDevices().at(i)->getJointStopMovementController())
+ JointController* jointCtrlOld =
+ controllersRequested.getWriteBuffer().jointControllers.at(i);
+ if (jointCtrlOld ==
+ _module<Devices>().getControlDevices().at(i)->getJointStopMovementController())
{
//don't replace this controller with emergency stop
jointCtrl = jointCtrlOld;
@@ -283,7 +324,10 @@ namespace armarx::RobotUnitModule
else
{
//no one controls this device -> emergency stop
- jointCtrl = _module<Devices>().getControlDevices().at(i)->getJointEmergencyStopController();
+ jointCtrl = _module<Devices>()
+ .getControlDevices()
+ .at(i)
+ ->getJointEmergencyStopController();
}
}
}
@@ -295,43 +339,61 @@ namespace armarx::RobotUnitModule
const auto& ctrl = request.nJointControllers.at(i);
if (ctrl)
{
- out << i << "\t'" << ctrl->getInstanceName() << "' \t of class '" << ctrl->getClassName() << "'\n";
+ out << i << "\t'" << ctrl->getInstanceName() << "' \t of class '"
+ << ctrl->getClassName() << "'\n";
}
}
};
writeRequestedControllers(std::move(request));
}
- void ControlThreadDataBuffer::updateVirtualRobot(const VirtualRobot::RobotPtr& robot, const std::vector<VirtualRobot::RobotNodePtr>& nodes) const
+ void
+ ControlThreadDataBuffer::updateVirtualRobot(
+ const VirtualRobot::RobotPtr& robot,
+ const std::vector<VirtualRobot::RobotNodePtr>& nodes) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- _module<Devices>().updateVirtualRobotFromSensorValues(robot, nodes, controlThreadOutputBuffer.getReadBuffer().sensors);
+ _module<Devices>().updateVirtualRobotFromSensorValues(
+ robot, nodes, controlThreadOutputBuffer.getReadBuffer().sensors);
}
- void ControlThreadDataBuffer::updateVirtualRobot(const VirtualRobot::RobotPtr& robot) const
+ void
+ ControlThreadDataBuffer::updateVirtualRobot(const VirtualRobot::RobotPtr& robot) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- _module<Devices>().updateVirtualRobotFromSensorValues(robot, controlThreadOutputBuffer.getReadBuffer().sensors);
+ _module<Devices>().updateVirtualRobotFromSensorValues(
+ robot, controlThreadOutputBuffer.getReadBuffer().sensors);
}
- void ControlThreadDataBuffer::_postFinishDeviceInitialization()
+
+ void
+ ControlThreadDataBuffer::_postFinishDeviceInitialization()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_DEBUG << "initializing buffers:";
{
- ARMARX_DEBUG << "----initializing controller buffers for " << _module<Devices>().getNumberOfControlDevices() << " control devices";
+ ARMARX_DEBUG << "----initializing controller buffers for "
+ << _module<Devices>().getNumberOfControlDevices() << " control devices";
JointAndNJointControllers ctrlinitReq(_module<Devices>().getNumberOfControlDevices());
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.jointControllers.size());
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.nJointControllers.size());
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.jointToNJointControllerAssignement.size());
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
+ ctrlinitReq.jointControllers.size());
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
+ ctrlinitReq.nJointControllers.size());
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
+ ctrlinitReq.jointToNJointControllerAssignement.size());
controllersActivated.reinitAllBuffers(ctrlinitReq);
- ctrlinitReq.jointControllers = DevicesAttorneyForControlThreadDataBuffer::GetStopMovementJointControllers(this);
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.jointControllers.size());
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.nJointControllers.size());
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.jointToNJointControllerAssignement.size());
+ ctrlinitReq.jointControllers =
+ DevicesAttorneyForControlThreadDataBuffer::GetStopMovementJointControllers(this);
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
+ ctrlinitReq.jointControllers.size());
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
+ ctrlinitReq.nJointControllers.size());
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
+ ctrlinitReq.jointToNJointControllerAssignement.size());
controllersRequested.reinitAllBuffers(ctrlinitReq);
- controllersRequested.commitWrite();//to make sure the diry bit is activated (to trigger rt switch)
+ controllersRequested
+ .commitWrite(); //to make sure the diry bit is activated (to trigger rt switch)
}
}
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h
index 66597aee2f9b7d14f597fa4fcaf0b2e533179db7..1833042321debe6dbd1487e9d5af4a673c670ac5 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h
@@ -28,11 +28,10 @@
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-#include "RobotUnitModuleBase.h"
-#include "../util/JointAndNJointControllers.h"
-#include "../util/ControlThreadOutputBuffer.h"
-
#include "../SensorValues/SensorValue1DoFActuator.h"
+#include "../util/ControlThreadOutputBuffer.h"
+#include "../util/JointAndNJointControllers.h"
+#include "RobotUnitModuleBase.h"
namespace armarx
{
@@ -80,15 +79,18 @@ namespace armarx::RobotUnitModule
class ControlThreadDataBuffer : virtual public ModuleBase
{
friend class ModuleBase;
+
public:
/**
* @brief Returns the singleton instance of this class
* @return The singleton instance of this class
*/
- static ControlThreadDataBuffer& Instance()
+ static ControlThreadDataBuffer&
+ Instance()
{
return ModuleBase::Instance<ControlThreadDataBuffer>();
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -109,7 +111,8 @@ namespace armarx::RobotUnitModule
* @param robot The robot to update
* @param nodes The robot's nodes
*/
- void updateVirtualRobot(const VirtualRobot::RobotPtr& robot, const std::vector<VirtualRobot::RobotNodePtr>& nodes) const;
+ void updateVirtualRobot(const VirtualRobot::RobotPtr& robot,
+ const std::vector<VirtualRobot::RobotNodePtr>& nodes) const;
/**
* @brief Returns whether the set of activated \ref JointController "Joint-" and
@@ -199,7 +202,8 @@ namespace armarx::RobotUnitModule
* \warning This function is dangerous to use.
* @return The \ref ControlThreadOutputBuffer
*/
- ControlThreadOutputBuffer& getControlThreadOutputBuffer() ///TODO refactor logging and remove this function
+ ControlThreadOutputBuffer&
+ getControlThreadOutputBuffer() ///TODO refactor logging and remove this function
{
return controlThreadOutputBuffer;
}
@@ -208,14 +212,17 @@ namespace armarx::RobotUnitModule
* @brief Returns the writer buffer for sensor and control values.
* @return The writer buffer for sensor and control values.
*/
- SensorAndControl& rtGetSensorAndControlBuffer() ///TODO move to attorney
+ SensorAndControl&
+ rtGetSensorAndControlBuffer() ///TODO move to attorney
{
return controlThreadOutputBuffer.getWriteBuffer();
}
+
/**
* @brief Commits the writer buffer for sensor and control values.
*/
- void rtSensorAndControlBufferCommitWrite() ///TODO move to attorney
+ void
+ rtSensorAndControlBufferCommitWrite() ///TODO move to attorney
{
return controlThreadOutputBuffer.commitWrite();
}
@@ -230,7 +237,8 @@ namespace armarx::RobotUnitModule
* @see NJointControllerBase::activateController
* @see NJointControllerBase::deactivateController
*/
- void setActivateControllersRequest(std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr> > ctrls);
+ void setActivateControllersRequest(
+ std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> ctrls);
// //////////////////////////////////////////////////////////////////////////////////////// //
// //////////////////////////////////// implementation //////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -242,7 +250,11 @@ namespace armarx::RobotUnitModule
* If any check fails, throw an exception with a descriptive message.
* @param controllers The controllers to activate
*/
- virtual void checkSetOfControllersToActivate(const JointAndNJointControllers& /*controllers*/) const {}
+ virtual void
+ checkSetOfControllersToActivate(const JointAndNJointControllers& /*controllers*/) const
+ {
+ }
+
private:
void writeRequestedControllers(JointAndNJointControllers&& setOfControllers);
@@ -268,7 +280,8 @@ namespace armarx::RobotUnitModule
/// \warning DO NOT ACCESS EXCEPT YOU KNOW WHAT YOU ARE DOING!
ControlThreadOutputBuffer controlThreadOutputBuffer;
/// @brief Mutex guarding \ref controlThreadOutputBuffer
- mutable std::recursive_mutex controlThreadOutputBufferMutex; /// TODO use this mutex instead of the global one
+ mutable std::recursive_mutex
+ controlThreadOutputBufferMutex; /// TODO use this mutex instead of the global one
// //////////////////////////////////////////////////////////////////////////////////////// //
// /////////////////////////////////////// Attorneys ////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -279,5 +292,4 @@ namespace armarx::RobotUnitModule
*/
friend class ControlThreadDataBufferAttorneyForControlThread;
};
-}
-
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
index 60ebf6dbf533c27820ee7b4b950e858004cac60f..6f0001391b69aa246edc781fee744997f214f988 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
@@ -23,23 +23,23 @@
#include "RobotUnitModuleControllerManagement.h"
+#include <ArmarXCore/core/ArmarXManager.h>
+#include <ArmarXCore/core/util/OnScopeExit.h>
+#include <ArmarXCore/core/util/algorithm.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
+
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h>
-#include <ArmarXCore/core/ArmarXManager.h>
-#include <ArmarXCore/core/util/OnScopeExit.h>
-#include <ArmarXCore/util/CPPUtility/trace.h>
-#include <ArmarXCore/core/util/algorithm.h>
-
namespace armarx::RobotUnitModule
{
- template<class Cont>
- static Ice::StringSeq GetNonNullNames(const Cont& c)
+ template <class Cont>
+ static Ice::StringSeq
+ GetNonNullNames(const Cont& c)
{
Ice::StringSeq result;
result.reserve(c.size());
@@ -60,42 +60,50 @@ namespace armarx::RobotUnitModule
class NJointControllerAttorneyForControllerManagement
{
friend class ControllerManagement;
- static void SetRequested(const NJointControllerBasePtr& nJointCtrl, bool requested)
+
+ static void
+ SetRequested(const NJointControllerBasePtr& nJointCtrl, bool requested)
{
nJointCtrl->isRequested = requested;
}
};
-}
+} // namespace armarx::RobotUnitModule
namespace armarx::RobotUnitModule
{
- Ice::StringSeq ControllerManagement::getRequestedNJointControllerNames(const Ice::Current&) const
+ Ice::StringSeq
+ ControllerManagement::getRequestedNJointControllerNames(const Ice::Current&) const
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return GetNonNullNames(_module<ControlThreadDataBuffer>().copyRequestedNJointControllers());
}
- Ice::StringSeq ControllerManagement::getActivatedNJointControllerNames(const Ice::Current&) const
+ Ice::StringSeq
+ ControllerManagement::getActivatedNJointControllerNames(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return GetNonNullNames(_module<ControlThreadDataBuffer>().getActivatedNJointControllers());
}
- void ControllerManagement::checkNJointControllerClassName(const std::string& className) const
+ void
+ ControllerManagement::checkNJointControllerClassName(const std::string& className) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (!NJointControllerRegistry::has(className))
{
std::stringstream ss;
- ss << "Requested controller class '" << className << "' unknown! Known classes:" << NJointControllerRegistry::getKeys()
- << " (If this class exists in a different lib then load it via loadLibFromPath(path) or loadLibFromPackage(package, lib))";
+ ss << "Requested controller class '" << className
+ << "' unknown! Known classes:" << NJointControllerRegistry::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()};
+ throw InvalidArgumentException{ss.str()};
}
}
- std::vector<NJointControllerBasePtr> ControllerManagement::getNJointControllersNotNull(const std::vector<std::string>& names) const
+ std::vector<NJointControllerBasePtr>
+ ControllerManagement::getNJointControllersNotNull(const std::vector<std::string>& names) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
@@ -109,7 +117,8 @@ namespace armarx::RobotUnitModule
return ctrl;
}
- const NJointControllerBasePtr& ControllerManagement::getNJointControllerNotNull(const std::string& name) const
+ const NJointControllerBasePtr&
+ ControllerManagement::getNJointControllerNotNull(const std::string& name) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
@@ -120,26 +129,29 @@ namespace armarx::RobotUnitModule
std::stringstream ss;
ss << "RobotUnit: there is no NJointControllerBase with name '" << name
<< "'. Existing NJointControllers are: " << getNJointControllerNames();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
if (!it->second)
{
std::stringstream ss;
ss << "RobotUnit: The NJointControllerBase with name '" << name
- << "'. Is a nullptr! This should never be the case (invariant)! \nMap:\n" << nJointControllers;
+ << "'. Is a nullptr! This should never be the case (invariant)! \nMap:\n"
+ << nJointControllers;
ARMARX_FATAL << ss.str();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
return it->second;
}
- void ControllerManagement::deleteNJointController(const NJointControllerBasePtr& ctrl)
+ void
+ ControllerManagement::deleteNJointController(const NJointControllerBasePtr& ctrl)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- deleteNJointControllers(std::vector<NJointControllerBasePtr> {ctrl});
+ deleteNJointControllers(std::vector<NJointControllerBasePtr>{ctrl});
}
- StringNJointControllerPrxDictionary ControllerManagement::getAllNJointControllers(const Ice::Current&) const
+ StringNJointControllerPrxDictionary
+ ControllerManagement::getAllNJointControllers(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::map<std::string, NJointControllerBasePtr> nJointControllersCopy;
@@ -151,24 +163,27 @@ namespace armarx::RobotUnitModule
StringNJointControllerPrxDictionary result;
for (const auto& pair : nJointControllersCopy)
{
- result[pair.first] = NJointControllerInterfacePrx::uncheckedCast(pair.second->getProxy(-1, true));
+ result[pair.first] =
+ NJointControllerInterfacePrx::uncheckedCast(pair.second->getProxy(-1, true));
}
return result;
}
- NJointControllerInterfacePrx ControllerManagement::createNJointController(
- const std::string& className,
- const std::string& instanceName,
- const NJointControllerConfigPtr& config,
- const Ice::Current&)
+ NJointControllerInterfacePrx
+ ControllerManagement::createNJointController(const std::string& className,
+ const std::string& instanceName,
+ const NJointControllerConfigPtr& config,
+ const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
//no lock required
return NJointControllerInterfacePrx::uncheckedCast(
- createNJointController(className, instanceName, config, true, false)->getProxy(-1, true));
+ createNJointController(className, instanceName, config, true, false)
+ ->getProxy(-1, true));
}
- NJointControllerInterfacePrx ControllerManagement::createNJointControllerFromVariantConfig(
+ NJointControllerInterfacePrx
+ ControllerManagement::createNJointControllerFromVariantConfig(
const std::string& className,
const std::string& instanceName,
const StringVariantBaseMap& variants,
@@ -180,24 +195,28 @@ namespace armarx::RobotUnitModule
if (!NJointControllerRegistry::get(className)->hasRemoteConfiguration())
{
std::stringstream ss;
- ss << "Requested controller class '" << className << "' allows no remote configuration" << NJointControllerRegistry::getKeys()
- << " (Implement 'static WidgetDescription::WidgetPtr " << className << "::GenerateConfigDescription()'"
+ ss << "Requested controller class '" << className << "' allows no remote configuration"
+ << NJointControllerRegistry::getKeys()
+ << " (Implement 'static WidgetDescription::WidgetPtr " << className
+ << "::GenerateConfigDescription()'"
<< " and 'static NJointControllerConfigPtr " << className
- << "::GenerateConfigFromVariants(const StringVariantBaseMap&)' to allow remote configuration";
+ << "::GenerateConfigFromVariants(const StringVariantBaseMap&)' to allow remote "
+ "configuration";
ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
return createNJointController(
- className, instanceName,
- NJointControllerRegistry::get(className)->GenerateConfigFromVariants(variants),
- Ice::emptyCurrent/*to select ice overload*/);
+ className,
+ instanceName,
+ NJointControllerRegistry::get(className)->GenerateConfigFromVariants(variants),
+ Ice::emptyCurrent /*to select ice overload*/);
}
- NJointControllerInterfacePrx ControllerManagement::createOrReplaceNJointController(
- const std::string& className,
- const std::string& instanceName,
- const NJointControllerConfigPtr& config,
- const Ice::Current&)
+ NJointControllerInterfacePrx
+ ControllerManagement::createOrReplaceNJointController(const std::string& className,
+ const std::string& instanceName,
+ const NJointControllerConfigPtr& config,
+ const Ice::Current&)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -232,15 +251,16 @@ namespace armarx::RobotUnitModule
ARMARX_INFO << "wiating until controller '" << instanceName << "' is removed from ice";
}
return NJointControllerInterfacePrx::uncheckedCast(
- createNJointController(className, instanceName, config, true, false)->getProxy(-1, true));
+ createNJointController(className, instanceName, config, true, false)
+ ->getProxy(-1, true));
}
- const NJointControllerBasePtr& ControllerManagement::createNJointController(
- const std::string& className,
- const std::string& instanceName,
- const NJointControllerConfigPtr& config,
- bool deletable,
- bool internal)
+ const NJointControllerBasePtr&
+ ControllerManagement::createNJointController(const std::string& className,
+ const std::string& instanceName,
+ const NJointControllerConfigPtr& config,
+ bool deletable,
+ bool internal)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
@@ -248,7 +268,7 @@ namespace armarx::RobotUnitModule
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)"};
+ throw InvalidArgumentException{"The instance name is empty! (give a unique name)"};
}
//check if we would be able to create the class
checkNJointControllerClassName(className);
@@ -258,24 +278,20 @@ namespace armarx::RobotUnitModule
if (nJointControllers.count(instanceName))
{
std::stringstream ss;
- ss << "There already is a controller instance with the name '" << instanceName << "'. Use a different instance name instead."
+ ss << "There already is a controller instance with the name '" << instanceName
+ << "'. Use a different instance name instead."
<< " Other used instance names are " << getNJointControllerNames();
ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
//create the controller
ARMARX_CHECK_EXPRESSION(factory);
- NJointControllerBasePtr nJointCtrl = factory->create(
- this,
- config,
- controllerCreateRobot,
- deletable, internal,
- instanceName
- );
- ARMARX_CHECK_NOT_EQUAL(
- nJointCtrl->getControlDeviceUsedIndices().size(), 0) <<
- "The NJointControllerBase '" << nJointCtrl->getName() << "' uses no ControlDevice! (It has to use at least one)";
+ NJointControllerBasePtr nJointCtrl =
+ factory->create(this, config, controllerCreateRobot, deletable, internal, instanceName);
+ ARMARX_CHECK_NOT_EQUAL(nJointCtrl->getControlDeviceUsedIndices().size(), 0)
+ << "The NJointControllerBase '" << nJointCtrl->getName()
+ << "' uses no ControlDevice! (It has to use at least one)";
getArmarXManager()->addObject(nJointCtrl, instanceName, false, false);
nJointControllers[instanceName] = std::move(nJointCtrl);
@@ -283,8 +299,8 @@ namespace armarx::RobotUnitModule
return nJointControllers.at(instanceName);
}
-
- bool ControllerManagement::loadLibFromPath(const std::string& path, const Ice::Current&)
+ bool
+ ControllerManagement::loadLibFromPath(const std::string& path, const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
const bool result = getArmarXManager()->loadLibFromPath(path);
@@ -292,7 +308,10 @@ namespace armarx::RobotUnitModule
return result;
}
- bool ControllerManagement::loadLibFromPackage(const std::string& package, const std::string& lib, const Ice::Current&)
+ bool
+ ControllerManagement::loadLibFromPackage(const std::string& package,
+ const std::string& lib,
+ const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
const bool result = getArmarXManager()->loadLibFromPackage(package, lib);
@@ -300,19 +319,24 @@ namespace armarx::RobotUnitModule
return result;
}
- Ice::StringSeq ControllerManagement::getNJointControllerClassNames(const Ice::Current&) const
+ Ice::StringSeq
+ ControllerManagement::getNJointControllerClassNames(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return NJointControllerRegistry::getKeys();
}
- Ice::StringSeq ControllerManagement::getNJointControllerNames(const Ice::Current&) const
+ Ice::StringSeq
+ ControllerManagement::getNJointControllerNames(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
return getMapKeys(nJointControllers);
}
- std::vector<std::string> ControllerManagement::getNJointControllerNames(const std::vector<NJointControllerBasePtr>& ctrls) const
+
+ std::vector<std::string>
+ ControllerManagement::getNJointControllerNames(
+ const std::vector<NJointControllerBasePtr>& ctrls) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::vector<std::string> result;
@@ -327,22 +351,31 @@ namespace armarx::RobotUnitModule
return result;
}
- void ControllerManagement::activateNJointController(const std::string& name, const Ice::Current&)
+ void
+ ControllerManagement::activateNJointController(const std::string& name, const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
activateNJointControllers(getNJointControllersNotNull({name}));
}
- void ControllerManagement::activateNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
+
+ void
+ ControllerManagement::activateNJointControllers(const Ice::StringSeq& names,
+ const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
activateNJointControllers(getNJointControllersNotNull(names));
}
- void ControllerManagement::activateNJointController(const NJointControllerBasePtr& ctrl)
+
+ void
+ ControllerManagement::activateNJointController(const NJointControllerBasePtr& ctrl)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- activateNJointControllers(std::vector<NJointControllerBasePtr> {ctrl});
+ activateNJointControllers(std::vector<NJointControllerBasePtr>{ctrl});
}
- void ControllerManagement::activateNJointControllers(const std::vector<NJointControllerBasePtr>& ctrlsToActVec)
+
+ void
+ ControllerManagement::activateNJointControllers(
+ const std::vector<NJointControllerBasePtr>& ctrlsToActVec)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (ctrlsToActVec.empty())
@@ -352,24 +385,21 @@ namespace armarx::RobotUnitModule
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
//if not activate them
- std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> ctrlsToAct {ctrlsToActVec.begin(), ctrlsToActVec.end()};
+ std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> ctrlsToAct{
+ ctrlsToActVec.begin(), ctrlsToActVec.end()};
ARMARX_CHECK_EXPRESSION(!ctrlsToAct.count(nullptr));
//check if all already active
- if (
- std::all_of(
- ctrlsToActVec.begin(), ctrlsToActVec.end(),
- [](const NJointControllerBasePtr & ctrl)
- {
- return ctrl->isControllerActive();
- }
- )
- )
+ if (std::all_of(ctrlsToActVec.begin(),
+ ctrlsToActVec.end(),
+ [](const NJointControllerBasePtr& ctrl)
+ { return ctrl->isControllerActive(); }))
{
return;
}
//get already requested
const auto ctrlVector = _module<ControlThreadDataBuffer>().copyRequestedNJointControllers();
- std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> ctrlsAlreadyRequested {ctrlVector.begin(), ctrlVector.end()};
+ std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>>
+ ctrlsAlreadyRequested{ctrlVector.begin(), ctrlVector.end()};
ctrlsAlreadyRequested.erase(nullptr);
//check for conflict
std::vector<char> inuse;
@@ -379,10 +409,11 @@ namespace armarx::RobotUnitModule
if (!r)
{
std::stringstream ss;
- ss << "activateNJointControllers: requested controllers are in conflict!\ncontrollers:\n"
+ ss << "activateNJointControllers: requested controllers are in "
+ "conflict!\ncontrollers:\n"
<< getNJointControllerNames(ctrlsToActVec);
ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
inuse = std::move(*r);
}
@@ -411,14 +442,16 @@ namespace armarx::RobotUnitModule
if (r)
{
ARMARX_DEBUG << "keeping already requested NJointControllerBase '"
- << nJointCtrl->getInstanceName() << "' in list of requested controllers";
+ << nJointCtrl->getInstanceName()
+ << "' in list of requested controllers";
ctrlsToAct.insert(nJointCtrl);
inuse = std::move(*r);
}
else
{
ARMARX_INFO << "removing already requested NJointControllerBase '"
- << nJointCtrl->getInstanceName() << "' from list of requested controllers";
+ << nJointCtrl->getInstanceName()
+ << "' from list of requested controllers";
}
}
ARMARX_DEBUG << "inuse field (all)\n" << printInUse;
@@ -426,22 +459,31 @@ namespace armarx::RobotUnitModule
_module<ControlThreadDataBuffer>().setActivateControllersRequest(ctrlsToAct);
}
- void ControllerManagement::deactivateNJointController(const std::string& name, const Ice::Current&)
+ void
+ ControllerManagement::deactivateNJointController(const std::string& name, const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deactivateNJointControllers(getNJointControllersNotNull({name}));
}
- void ControllerManagement::deactivateNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
+
+ void
+ ControllerManagement::deactivateNJointControllers(const Ice::StringSeq& names,
+ const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deactivateNJointControllers(getNJointControllersNotNull(names));
}
- void ControllerManagement::deactivateNJointController(const NJointControllerBasePtr& ctrl)
+
+ void
+ ControllerManagement::deactivateNJointController(const NJointControllerBasePtr& ctrl)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- deactivateNJointControllers(std::vector<NJointControllerBasePtr> {ctrl});
+ deactivateNJointControllers(std::vector<NJointControllerBasePtr>{ctrl});
}
- void ControllerManagement::deactivateNJointControllers(const std::vector<NJointControllerBasePtr>& ctrlsDeacVec)
+
+ void
+ ControllerManagement::deactivateNJointControllers(
+ const std::vector<NJointControllerBasePtr>& ctrlsDeacVec)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
@@ -451,7 +493,8 @@ namespace armarx::RobotUnitModule
return;
}
const auto ctrlVector = _module<ControlThreadDataBuffer>().copyRequestedNJointControllers();
- std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> ctrls {ctrlVector.begin(), ctrlVector.end()};
+ std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> ctrls{
+ ctrlVector.begin(), ctrlVector.end()};
const std::size_t ctrlsNum = ctrls.size();
for (const auto& nJointCtrlToDeactivate : ctrlsDeacVec)
{
@@ -464,25 +507,36 @@ namespace armarx::RobotUnitModule
_module<ControlThreadDataBuffer>().setActivateControllersRequest(ctrls);
}
- void ControllerManagement::deleteNJointController(const std::string& name, const Ice::Current&)
+ void
+ ControllerManagement::deleteNJointController(const std::string& name, const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deleteNJointControllers(getNJointControllersNotNull({name}));
}
- void ControllerManagement::deleteNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
+
+ void
+ ControllerManagement::deleteNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deleteNJointControllers(getNJointControllersNotNull(names));
}
- void ControllerManagement::switchNJointControllerSetup(const Ice::StringSeq& newSetup, const Ice::Current&)
+
+ void
+ ControllerManagement::switchNJointControllerSetup(const Ice::StringSeq& newSetup,
+ const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
- auto ctrlsToActVec = getNJointControllersNotNull(newSetup); //also checks if these controllers exist
- _module<ControlThreadDataBuffer>().setActivateControllersRequest({ctrlsToActVec.begin(), ctrlsToActVec.end()});
+ auto ctrlsToActVec =
+ getNJointControllersNotNull(newSetup); //also checks if these controllers exist
+ _module<ControlThreadDataBuffer>().setActivateControllersRequest(
+ {ctrlsToActVec.begin(), ctrlsToActVec.end()});
}
- void ControllerManagement::deleteNJointControllers(const std::vector<NJointControllerBasePtr>& ctrlsToDelVec)
+
+ void
+ ControllerManagement::deleteNJointControllers(
+ const std::vector<NJointControllerBasePtr>& ctrlsToDelVec)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
@@ -496,19 +550,15 @@ namespace armarx::RobotUnitModule
{
if (!nJointCtrl->isDeletable())
{
- throw LogicError
- {
- "The NJointControllerBase '" + nJointCtrl->getInstanceName() +
- "' can't be deleted since this operation is not allowed for this controller! (no NJointControllerBase was deleted)"
- };
+ throw LogicError{"The NJointControllerBase '" + nJointCtrl->getInstanceName() +
+ "' can't be deleted since this operation is not allowed for this "
+ "controller! (no NJointControllerBase was deleted)"};
}
if (nJointCtrl->isControllerActive() || nJointCtrl->isControllerRequested())
{
- throw LogicError
- {
- "The NJointControllerBase '" + nJointCtrl->getInstanceName() +
- "' can't be deleted since it is active or requested! (no NJointControllerBase was deleted)"
- };
+ throw LogicError{"The NJointControllerBase '" + nJointCtrl->getInstanceName() +
+ "' can't be deleted since it is active or requested! (no "
+ "NJointControllerBase was deleted)"};
}
}
for (const auto& nJointCtrl : ctrlsToDelVec)
@@ -521,22 +571,32 @@ namespace armarx::RobotUnitModule
}
}
- void ControllerManagement::deactivateAndDeleteNJointController(const std::string& name, const Ice::Current&)
+ void
+ ControllerManagement::deactivateAndDeleteNJointController(const std::string& name,
+ const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deactivateAndDeleteNJointControllers(getNJointControllersNotNull({name}));
}
- void ControllerManagement::deactivateAndDeleteNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
+
+ void
+ ControllerManagement::deactivateAndDeleteNJointControllers(const Ice::StringSeq& names,
+ const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deactivateAndDeleteNJointControllers(getNJointControllersNotNull(names));
}
- void ControllerManagement::deactivateAndDeleteNJointController(const NJointControllerBasePtr& ctrl)
+
+ void
+ ControllerManagement::deactivateAndDeleteNJointController(const NJointControllerBasePtr& ctrl)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deactivateAndDeleteNJointControllers({ctrl});
}
- void ControllerManagement::deactivateAndDeleteNJointControllers(const std::vector<NJointControllerBasePtr>& ctrlsToDelVec)
+
+ void
+ ControllerManagement::deactivateAndDeleteNJointControllers(
+ const std::vector<NJointControllerBasePtr>& ctrlsToDelVec)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
@@ -546,31 +606,27 @@ namespace armarx::RobotUnitModule
return;
}
deactivateNJointControllers(ctrlsToDelVec);
- while (
- std::any_of(
- ctrlsToDelVec.begin(),
- ctrlsToDelVec.end(),
- [](const NJointControllerBasePtr & ctrl)
- {
- return ctrl->isControllerActive();
- }
- )
- )
+ while (std::any_of(ctrlsToDelVec.begin(),
+ ctrlsToDelVec.end(),
+ [](const NJointControllerBasePtr& ctrl)
+ { return ctrl->isControllerActive(); }))
{
if (isShuttingDown())
{
return;
}
- std::this_thread::sleep_for(std::chrono::microseconds {100});
+ std::this_thread::sleep_for(std::chrono::microseconds{100});
}
deleteNJointControllers(ctrlsToDelVec);
}
- NJointControllerClassDescription ControllerManagement::getNJointControllerClassDescription(
- const std::string& className, const Ice::Current&) const
+ NJointControllerClassDescription
+ ControllerManagement::getNJointControllerClassDescription(const std::string& className,
+ const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- while (getRobotUnitState() == RobotUnitState::InitializingComponent || getRobotUnitState() == RobotUnitState::InitializingDevices)
+ while (getRobotUnitState() == RobotUnitState::InitializingComponent ||
+ getRobotUnitState() == RobotUnitState::InitializingDevices)
{
//this phase should only last short so busy waiting is ok
std::this_thread::sleep_for(std::chrono::milliseconds(50));
@@ -586,23 +642,25 @@ namespace armarx::RobotUnitModule
NJointControllerRegistry::get(className)->GenerateConfigDescription(
controllerCreateRobot,
_module<Devices>().getControlDevicesConstPtr(),
- _module<Devices>().getSensorDevicesConstPtr()
- );
+ _module<Devices>().getSensorDevicesConstPtr());
}
return data;
}
- NJointControllerClassDescriptionSeq ControllerManagement::getNJointControllerClassDescriptions(const Ice::Current&) const
+ NJointControllerClassDescriptionSeq
+ ControllerManagement::getNJointControllerClassDescriptions(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::size_t tries = 200;
- while (getRobotUnitState() == RobotUnitState::InitializingComponent || getRobotUnitState() == RobotUnitState::InitializingDevices)
+ while (getRobotUnitState() == RobotUnitState::InitializingComponent ||
+ getRobotUnitState() == RobotUnitState::InitializingDevices)
{
//this phase should only last short so busy waiting is ok
std::this_thread::sleep_for(std::chrono::milliseconds(50));
- if (! --tries)
+ if (!--tries)
{
- throw RuntimeError {"RobotUnit::getNJointControllerClassDescriptions: it took too long to for the unit to get in a valid state"};
+ throw RuntimeError{"RobotUnit::getNJointControllerClassDescriptions: it took too "
+ "long to for the unit to get in a valid state"};
}
}
auto guard = getGuard();
@@ -615,7 +673,8 @@ namespace armarx::RobotUnitModule
return r;
}
- NJointControllerInterfacePrx ControllerManagement::getNJointController(const std::string& name, const Ice::Current&) const
+ NJointControllerInterfacePrx
+ ControllerManagement::getNJointController(const std::string& name, const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
NJointControllerBasePtr ctrl;
@@ -631,7 +690,9 @@ namespace armarx::RobotUnitModule
return NJointControllerInterfacePrx::uncheckedCast(ctrl->getProxy(-1, true));
}
- NJointControllerStatus ControllerManagement::getNJointControllerStatus(const std::string& name, const Ice::Current&) const
+ NJointControllerStatus
+ ControllerManagement::getNJointControllerStatus(const std::string& name,
+ const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
@@ -639,7 +700,8 @@ namespace armarx::RobotUnitModule
return getNJointControllerNotNull(name)->getControllerStatus();
}
- NJointControllerStatusSeq ControllerManagement::getNJointControllerStatuses(const Ice::Current&) const
+ NJointControllerStatusSeq
+ ControllerManagement::getNJointControllerStatuses(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
@@ -656,7 +718,9 @@ namespace armarx::RobotUnitModule
return r;
}
- NJointControllerDescription ControllerManagement::getNJointControllerDescription(const std::string& name, const Ice::Current&) const
+ NJointControllerDescription
+ ControllerManagement::getNJointControllerDescription(const std::string& name,
+ const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
NJointControllerBasePtr nJointCtrl;
@@ -668,7 +732,8 @@ namespace armarx::RobotUnitModule
return nJointCtrl->getControllerDescription();
}
- NJointControllerDescriptionSeq ControllerManagement::getNJointControllerDescriptions(const Ice::Current&) const
+ NJointControllerDescriptionSeq
+ ControllerManagement::getNJointControllerDescriptions(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::map<std::string, NJointControllerBasePtr> nJointControllersCopy;
@@ -689,8 +754,9 @@ namespace armarx::RobotUnitModule
return r;
}
- NJointControllerDescriptionWithStatus ControllerManagement::getNJointControllerDescriptionWithStatus(
- const std::string& name, const Ice::Current&) const
+ NJointControllerDescriptionWithStatus
+ ControllerManagement::getNJointControllerDescriptionWithStatus(const std::string& name,
+ const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
NJointControllerBasePtr nJointCtrl;
@@ -702,7 +768,8 @@ namespace armarx::RobotUnitModule
return nJointCtrl->getControllerDescriptionWithStatus();
}
- NJointControllerDescriptionWithStatusSeq ControllerManagement::getNJointControllerDescriptionsWithStatuses(const Ice::Current&) const
+ NJointControllerDescriptionWithStatusSeq
+ ControllerManagement::getNJointControllerDescriptionsWithStatuses(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::map<std::string, NJointControllerBasePtr> nJointControllersCopy;
@@ -723,7 +790,11 @@ namespace armarx::RobotUnitModule
return r;
}
- void ControllerManagement::removeNJointControllers(std::map<std::string, NJointControllerBasePtr>& ctrls, bool blocking, RobotUnitListenerPrx l)
+ void
+ ControllerManagement::removeNJointControllers(
+ std::map<std::string, NJointControllerBasePtr>& ctrls,
+ bool blocking,
+ RobotUnitListenerPrx l)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
for (auto& n2NJointCtrl : ctrls)
@@ -747,13 +818,15 @@ namespace armarx::RobotUnitModule
ctrls.clear();
}
- void ControllerManagement::removeNJointControllersToBeDeleted(bool blocking, RobotUnitListenerPrx l)
+ void
+ ControllerManagement::removeNJointControllersToBeDeleted(bool blocking, RobotUnitListenerPrx l)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
removeNJointControllers(nJointControllersToBeDeleted, blocking, l);
}
- void ControllerManagement::_preFinishRunning()
+ void
+ ControllerManagement::_preFinishRunning()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
//NJoint queued for deletion (some could still be in the queue)
@@ -766,7 +839,8 @@ namespace armarx::RobotUnitModule
ARMARX_DEBUG << "remove NJointControllers...done";
}
- void ControllerManagement::_postFinishRunning()
+ void
+ ControllerManagement::_postFinishRunning()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
nJointControllers.clear();
@@ -774,22 +848,25 @@ namespace armarx::RobotUnitModule
ControllerManagement::~ControllerManagement()
{
-
}
- void ControllerManagement::_preOnInitRobotUnit()
+ void
+ ControllerManagement::_preOnInitRobotUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
controllerCreateRobot = _module<RobotData>().cloneRobot();
}
- void ControllerManagement::updateNJointControllerRequestedState(const std::set<NJointControllerBasePtr>& request)
+ void
+ ControllerManagement::updateNJointControllerRequestedState(
+ const std::set<NJointControllerBasePtr>& request)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_DEBUG << "set requested state for NJoint controllers";
for (const auto& name2NJoint : nJointControllers)
{
- NJointControllerAttorneyForControllerManagement::SetRequested(name2NJoint.second, request.count(name2NJoint.second));
+ NJointControllerAttorneyForControllerManagement::SetRequested(
+ name2NJoint.second, request.count(name2NJoint.second));
}
}
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h
index 3317540bc5e9adf326ba8410d06df2576d907ec3..9541d6c791b508c7c4b844803455cf69c7bee55f 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h
@@ -36,15 +36,19 @@ namespace armarx::RobotUnitModule
*
* @see ModuleBase
*/
- class ControllerManagement : virtual public ModuleBase, virtual public RobotUnitControllerManagementInterface
+ class ControllerManagement :
+ virtual public ModuleBase,
+ virtual public RobotUnitControllerManagementInterface
{
friend class ModuleBase;
+
public:
/**
* @brief Returns the singleton instance of this class
* @return The singleton instance of this class
*/
- static ControllerManagement& Instance()
+ static ControllerManagement&
+ Instance()
{
return ModuleBase::Instance<ControllerManagement>();
}
@@ -71,13 +75,16 @@ namespace armarx::RobotUnitModule
* @return A proxy to the \ref NJointControllerBase.
* @see getAllNJointControllers
*/
- NJointControllerInterfacePrx getNJointController(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ NJointControllerInterfacePrx
+ getNJointController(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns proxies to all \ref NJointControllerBase "NJointControllers"
* @return Proxies to all \ref NJointControllerBase "NJointControllers"
* @see getNJointController
*/
- StringNJointControllerPrxDictionary getAllNJointControllers(const Ice::Current& = Ice::emptyCurrent) const override;
+ StringNJointControllerPrxDictionary
+ getAllNJointControllers(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the status of the \ref NJointControllerBase.
@@ -88,7 +95,9 @@ namespace armarx::RobotUnitModule
* @see getNJointControllerDescriptionWithStatus
* @see getNJointControllerDescriptionsWithStatuses
*/
- NJointControllerStatus getNJointControllerStatus(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ NJointControllerStatus
+ getNJointControllerStatus(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the status of all \ref NJointControllerBase "NJointControllers".
* @return The status of all \ref NJointControllerBase "NJointControllers".
@@ -97,7 +106,8 @@ namespace armarx::RobotUnitModule
* @see getNJointControllerDescriptionWithStatus
* @see getNJointControllerDescriptionsWithStatuses
*/
- NJointControllerStatusSeq getNJointControllerStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
+ NJointControllerStatusSeq
+ getNJointControllerStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the description of the \ref NJointControllerBase.
@@ -108,7 +118,9 @@ namespace armarx::RobotUnitModule
* @see getNJointControllerDescriptionWithStatus
* @see getNJointControllerDescriptionsWithStatuses
*/
- NJointControllerDescription getNJointControllerDescription(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ NJointControllerDescription
+ getNJointControllerDescription(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the description of all \ref NJointControllerBase "NJointControllers".
* @return The description of all \ref NJointControllerBase "NJointControllers".
@@ -118,7 +130,8 @@ namespace armarx::RobotUnitModule
* @see getNJointControllerDescriptionsWithStatuses
*/
- NJointControllerDescriptionSeq getNJointControllerDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
+ NJointControllerDescriptionSeq
+ getNJointControllerDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the status and description of the \ref NJointControllerBase.
@@ -133,7 +146,8 @@ namespace armarx::RobotUnitModule
* @see getNJointControllerDescriptions
*/
NJointControllerDescriptionWithStatus getNJointControllerDescriptionWithStatus(
- const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the status and description of all \ref NJointControllerBase "NJointControllers".
* @return The status and description of all \ref NJointControllerBase "NJointControllers".
@@ -145,27 +159,31 @@ namespace armarx::RobotUnitModule
* @see getNJointControllerDescription
* @see getNJointControllerDescriptions
*/
- NJointControllerDescriptionWithStatusSeq getNJointControllerDescriptionsWithStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
+ NJointControllerDescriptionWithStatusSeq getNJointControllerDescriptionsWithStatuses(
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief getNJointControllerClassDescription
* @param className
* @return
*/
- NJointControllerClassDescription getNJointControllerClassDescription(
- const std::string& className, const Ice::Current& = Ice::emptyCurrent) const override;
+ NJointControllerClassDescription
+ getNJointControllerClassDescription(const std::string& className,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief getNJointControllerClassDescriptions
* @return
*/
- NJointControllerClassDescriptionSeq getNJointControllerClassDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
+ NJointControllerClassDescriptionSeq getNJointControllerClassDescriptions(
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Loads the given lib. (calls `getArmarXManager()->loadLibFromPath(path)`)
* @param path Path to the lib to load.
* @return Whether loading the lib was successful.
* @see ArmarXManager::loadLibFromPath
*/
- bool loadLibFromPath(const std::string& path, const Ice::Current& = Ice::emptyCurrent) override;
+ bool loadLibFromPath(const std::string& path,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Loads the given lib. (calls `getArmarXManager()->loadLibFromPath(package, lib)`)
* @param package The armarx package containing the lib
@@ -173,27 +191,33 @@ namespace armarx::RobotUnitModule
* @return Whether loading the lib was successful.
* @see ArmarXManager::loadLibFromPackage
*/
- bool loadLibFromPackage(const std::string& package, const std::string& lib, const Ice::Current& = Ice::emptyCurrent) override;
+ bool loadLibFromPackage(const std::string& package,
+ const std::string& lib,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Returns the names of all available classes of \ref NJointControllerBase.
* @return The names of all available classes of \ref NJointControllerBase.
*/
- Ice::StringSeq getNJointControllerClassNames(const Ice::Current& = Ice::emptyCurrent) const override;
+ Ice::StringSeq
+ getNJointControllerClassNames(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the names of all \ref NJointControllerBase "NJointControllers"
* @return The names of all \ref NJointControllerBase "NJointControllers"
*/
- Ice::StringSeq getNJointControllerNames(const Ice::Current& = Ice::emptyCurrent) const override;
+ Ice::StringSeq
+ getNJointControllerNames(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the names of all requested \ref NJointControllerBase "NJointControllers"
* @return The names of all requested \ref NJointControllerBase "NJointControllers"
*/
- Ice::StringSeq getRequestedNJointControllerNames(const Ice::Current& = Ice::emptyCurrent) const override;
+ Ice::StringSeq
+ getRequestedNJointControllerNames(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the names of all activated \ref NJointControllerBase "NJointControllers"
* @return The names of all activated \ref NJointControllerBase "NJointControllers"
*/
- Ice::StringSeq getActivatedNJointControllerNames(const Ice::Current& = Ice::emptyCurrent) const override;
+ Ice::StringSeq
+ getActivatedNJointControllerNames(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Queues the given \ref NJointControllerBase for deletion.
@@ -202,7 +226,8 @@ namespace armarx::RobotUnitModule
* @see nJointControllersToBeDeleted
* @see deleteNJointControllers
*/
- void deleteNJointController(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void deleteNJointController(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Queues the given \ref NJointControllerBase "NJointControllers" for deletion.
* @param names The \ref NJointControllerBase "NJointControllers" to delete.
@@ -210,7 +235,8 @@ namespace armarx::RobotUnitModule
* @see nJointControllersToBeDeleted
* @see deleteNJointController
*/
- void deleteNJointControllers(const Ice::StringSeq& names, const Ice::Current& = Ice::emptyCurrent) override;
+ void deleteNJointControllers(const Ice::StringSeq& names,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Queues the given \ref NJointControllerBase for deletion and deactivates it if necessary.
* @param name The \ref NJointControllerBase to delete.
@@ -220,7 +246,8 @@ namespace armarx::RobotUnitModule
* @see deleteNJointControllers
* @see deactivateAnddeleteNJointControllers
*/
- void deactivateAndDeleteNJointController(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void deactivateAndDeleteNJointController(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Queues the given \ref NJointControllerBase "NJointControllers" for deletion and deactivates them if necessary.
* @param names The \ref NJointControllerBase "NJointControllers" to delete.
@@ -230,33 +257,38 @@ namespace armarx::RobotUnitModule
* @see deleteNJointControllers
* @see deactivateAnddeleteNJointController
*/
- void deactivateAndDeleteNJointControllers(const Ice::StringSeq& names, const Ice::Current&) override;
+ void deactivateAndDeleteNJointControllers(const Ice::StringSeq& names,
+ const Ice::Current&) override;
/**
* @brief Requests activation for the given \ref NJointControllerBase.
* @param name The requested \ref NJointControllerBase.
* @see activateNJointControllers
*/
- void activateNJointController(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void activateNJointController(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Requests activation for the given \ref NJointControllerBase "NJointControllers".
* @param names The requested \ref NJointControllerBase "NJointControllers".
* @see activateNJointController
*/
- void activateNJointControllers(const Ice::StringSeq& names, const Ice::Current& = Ice::emptyCurrent) override;
+ void activateNJointControllers(const Ice::StringSeq& names,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Requests deactivation for the given \ref NJointControllerBase.
* @param name The \ref NJointControllerBase to be deactivated.
* @see deactivateNJointControllers
*/
- void deactivateNJointController(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void deactivateNJointController(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Requests deactivation for the given \ref NJointControllerBase "NJointControllers".
* @param names The \ref NJointControllerBase "NJointControllers" to be deactivated.
* @see deactivateNJointController
*/
- void deactivateNJointControllers(const Ice::StringSeq& names, const Ice::Current& = Ice::emptyCurrent) override;
+ void deactivateNJointControllers(const Ice::StringSeq& names,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Cretes a \ref NJointControllerBase.
@@ -265,9 +297,11 @@ namespace armarx::RobotUnitModule
* @param config A config passed to the \ref NJointControllerBase's ctor.
* @return A proxy to the created \ref NJointControllerBase.
*/
- NJointControllerInterfacePrx createNJointController(
- const std::string& className, const std::string& instanceName,
- const NJointControllerConfigPtr& config, const Ice::Current& = Ice::emptyCurrent) override;
+ NJointControllerInterfacePrx
+ createNJointController(const std::string& className,
+ const std::string& instanceName,
+ const NJointControllerConfigPtr& config,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Cretes a \ref NJointControllerBase.
* @param className The \ref NJointControllerBase's class.
@@ -275,9 +309,11 @@ namespace armarx::RobotUnitModule
* @param variants A map of \ref Variant "Variants" passed to the \ref NJointControllerBase's ctor.
* @return A proxy to the created \ref NJointControllerBase.
*/
- NJointControllerInterfacePrx createNJointControllerFromVariantConfig(
- const std::string& className, const std::string& instanceName,
- const StringVariantBaseMap& variants, const Ice::Current& = Ice::emptyCurrent) override;
+ NJointControllerInterfacePrx
+ createNJointControllerFromVariantConfig(const std::string& className,
+ const std::string& instanceName,
+ const StringVariantBaseMap& variants,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Deletes any \ref NJointControllerBase with the given name and creates a new one.
* @param className The \ref NJointControllerBase's class.
@@ -285,15 +321,18 @@ namespace armarx::RobotUnitModule
* @param config A config passed to the \ref NJointControllerBase's ctor.
* @return A proxy to the created \ref NJointControllerBase.
*/
- NJointControllerInterfacePrx createOrReplaceNJointController(
- const std::string& className, const std::string& instanceName,
- const NJointControllerConfigPtr& config, const Ice::Current& = Ice::emptyCurrent) override;
+ NJointControllerInterfacePrx
+ createOrReplaceNJointController(const std::string& className,
+ const std::string& instanceName,
+ const NJointControllerConfigPtr& config,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Changes the set of requested \ref NJointControllerBase "NJointControllers" to the given set.
* @param newSetup The new set of requested \ref NJointControllerBase "NJointControllers"
*/
- void switchNJointControllerSetup(const Ice::StringSeq& newSetup, const Ice::Current& = Ice::emptyCurrent) override;
+ void switchNJointControllerSetup(const Ice::StringSeq& newSetup,
+ const Ice::Current& = Ice::emptyCurrent) override;
// //////////////////////////////////////////////////////////////////////////////////////// //
// /////////////////////////////////// Module interface /////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -307,9 +346,12 @@ namespace armarx::RobotUnitModule
* @param internal Whether the \ref NJointControllerBase should be tagged as internal.
* @return A pointer to the created \ref NJointControllerBase
*/
- const NJointControllerBasePtr& createNJointController(
- const std::string& className, const std::string& instanceName,
- const NJointControllerConfigPtr& config, bool deletable, bool internal);
+ const NJointControllerBasePtr&
+ createNJointController(const std::string& className,
+ const std::string& instanceName,
+ const NJointControllerConfigPtr& config,
+ bool deletable,
+ bool internal);
/**
* @brief Returns pointers to the \ref NJointControllerBase "NJointControllers". (If one does not exist, an exception is thrown.)
@@ -317,7 +359,8 @@ namespace armarx::RobotUnitModule
* @return Pointers to the \ref NJointControllerBase "NJointControllers".
* @throw If there is no \ref NJointControllerBase for \param name
*/
- std::vector<armarx::NJointControllerBasePtr> getNJointControllersNotNull(const std::vector<std::string>& names) const;
+ std::vector<armarx::NJointControllerBasePtr>
+ getNJointControllersNotNull(const std::vector<std::string>& names) const;
/**
* @brief Returns a pointer to the \ref NJointControllerBase. (If it does not exist, an exception is thrown.)
* @param name The \ref NJointControllerBase
@@ -331,7 +374,8 @@ namespace armarx::RobotUnitModule
* @param ctrls The \ref NJointControllerBase "NJointControllers"
* @return The names of given \ref NJointControllerBase "NJointControllers"
*/
- std::vector<std::string> getNJointControllerNames(const std::vector<armarx::NJointControllerBasePtr>& ctrls) const;
+ std::vector<std::string>
+ getNJointControllerNames(const std::vector<armarx::NJointControllerBasePtr>& ctrls) const;
/**
* @brief Queues the given \ref NJointControllerBase for deletion.
@@ -368,7 +412,8 @@ namespace armarx::RobotUnitModule
* @see deleteNJointControllers
* @see deactivateAnddeleteNJointController
*/
- void deactivateAndDeleteNJointControllers(const std::vector<NJointControllerBasePtr>& ctrls);
+ void
+ deactivateAndDeleteNJointControllers(const std::vector<NJointControllerBasePtr>& ctrls);
/**
@@ -408,7 +453,9 @@ namespace armarx::RobotUnitModule
* @param blocking Whether removal from the \ref ArmarXManager should be blocking.
* @param l Proxy to the listener topic all removal events should be sent to.
*/
- void removeNJointControllers(std::map<std::string, NJointControllerBasePtr>& ctrls, bool blocking = true, RobotUnitListenerPrx l = nullptr);
+ void removeNJointControllers(std::map<std::string, NJointControllerBasePtr>& ctrls,
+ bool blocking = true,
+ RobotUnitListenerPrx l = nullptr);
/**
* @brief Calls \ref removeNJointControllers for all \ref NJointControllerBase "NJointControllers"
* queued for deletion (\ref nJointControllersToBeDeleted).
@@ -417,7 +464,8 @@ namespace armarx::RobotUnitModule
* @see removeNJointControllers
* @see nJointControllersToBeDeleted
*/
- void removeNJointControllersToBeDeleted(bool blocking = true, RobotUnitListenerPrx l = nullptr);
+ void removeNJointControllersToBeDeleted(bool blocking = true,
+ RobotUnitListenerPrx l = nullptr);
/**
* @brief Sets the requested flag for all given \ref NJointControllerBase "NJointControllers" and unsets it for the rest.
@@ -458,4 +506,4 @@ namespace armarx::RobotUnitModule
*/
friend class ControllerManagementAttorneyForControlThreadDataBuffer;
};
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
index c29c7ce2d74f3f4ee7f7dc04cc96e48b46315f89..e9aea0a9858d85e24b9e49ee088c18405af0668a 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
@@ -21,40 +21,44 @@
*/
#include "RobotUnitModuleDevices.h"
-#include "RobotUnitModuleControlThreadDataBuffer.h"
-#include "RobotUnitModuleRobotData.h"
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/core/util/algorithm.h>
+
#include "RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h"
+#include "RobotUnitModuleControlThreadDataBuffer.h"
+#include "RobotUnitModuleRobotData.h"
+
namespace armarx::RobotUnitModule
{
const std::string Devices::rtThreadTimingsSensorDeviceName = "RTThreadTimings";
- ControlDeviceDescription Devices::getControlDeviceDescription(const std::string& name, const Ice::Current&) const
+ ControlDeviceDescription
+ Devices::getControlDeviceDescription(const std::string& name, const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
if (!controlDevices.has(name))
{
std::stringstream ss;
ss << "getControlDeviceDescription: There is no ControlDevice '" << name
<< "'. There are these ControlDevices: " << controlDevices.keys();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
return getControlDeviceDescription(controlDevices.index(name));
}
- ControlDeviceDescriptionSeq Devices::getControlDeviceDescriptions(const Ice::Current&) const
+ ControlDeviceDescriptionSeq
+ Devices::getControlDeviceDescriptions(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (!areDevicesReady())
{
return {};
}
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
ControlDeviceDescriptionSeq r;
r.reserve(getNumberOfControlDevices());
for (auto idx : getIndices(controlDevices.values()))
@@ -65,124 +69,145 @@ namespace armarx::RobotUnitModule
return r;
}
- std::size_t Devices::getNumberOfControlDevices() const
+ std::size_t
+ Devices::getNumberOfControlDevices() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
return controlDevices.size();
}
- std::size_t Devices::getNumberOfSensorDevices() const
+ std::size_t
+ Devices::getNumberOfSensorDevices() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
return sensorDevices.size();
}
- std::size_t Devices::getSensorDeviceIndex(const std::string& deviceName) const
+ std::size_t
+ Devices::getSensorDeviceIndex(const std::string& deviceName) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ std::lock_guard<MutexType> guard{sensorDevicesMutex};
ARMARX_CHECK_EXPRESSION(sensorDevices.has(deviceName));
return sensorDevices.index(deviceName);
}
- std::size_t Devices::getControlDeviceIndex(const std::string& deviceName) const
+
+ std::size_t
+ Devices::getControlDeviceIndex(const std::string& deviceName) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
ARMARX_CHECK_EXPRESSION(controlDevices.has(deviceName));
return controlDevices.index(deviceName);
}
- ConstSensorDevicePtr Devices::getSensorDevice(const std::string& sensorDeviceName) const
+ ConstSensorDevicePtr
+ Devices::getSensorDevice(const std::string& sensorDeviceName) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ std::lock_guard<MutexType> guard{sensorDevicesMutex};
return sensorDevices.at(sensorDeviceName, SensorDevice::NullPtr);
}
- ConstControlDevicePtr Devices::getControlDevice(const std::string& deviceName) const
+ ConstControlDevicePtr
+ Devices::getControlDevice(const std::string& deviceName) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
return controlDevices.at(deviceName, ControlDevice::NullPtr);
}
- Ice::StringSeq Devices::getControlDeviceNames(const Ice::Current&) const
+ Ice::StringSeq
+ Devices::getControlDeviceNames(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
return controlDevices.keys();
}
- ControlDeviceDescription Devices::getControlDeviceDescription(std::size_t idx) const
+ ControlDeviceDescription
+ Devices::getControlDeviceDescription(std::size_t idx) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
const ControlDevicePtr& controlDevice = controlDevices.at(idx);
ControlDeviceDescription data;
data.deviceName = controlDevice->getDeviceName();
data.tags.assign(controlDevice->getTags().begin(), controlDevice->getTags().end());
for (const auto& jointCtrl : controlDevice->getJointControllers())
{
- data.contolModeToTargetType[jointCtrl->getControlMode()].targetType = jointCtrl->getControlTarget()->getControlTargetType();
- data.contolModeToTargetType[jointCtrl->getControlMode()].hardwareControlMode = jointCtrl->getHardwareControlMode();
+ data.contolModeToTargetType[jointCtrl->getControlMode()].targetType =
+ jointCtrl->getControlTarget()->getControlTargetType();
+ data.contolModeToTargetType[jointCtrl->getControlMode()].hardwareControlMode =
+ jointCtrl->getHardwareControlMode();
}
throwIfDevicesNotReady(__FUNCTION__);
return data;
}
- ControlDeviceStatus Devices::getControlDeviceStatus(std::size_t idx) const
+ ControlDeviceStatus
+ Devices::getControlDeviceStatus(std::size_t idx) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
const ControlDevicePtr& controlDevice = controlDevices.at(idx);
ControlDeviceStatus status;
- const auto activeJointCtrl = _module<ControlThreadDataBuffer>().getActivatedJointControllers().at(idx);
- status.activeControlMode = activeJointCtrl ? activeJointCtrl->getControlMode() : std::string {"!!JointController is nullptr!!"};
+ const auto activeJointCtrl =
+ _module<ControlThreadDataBuffer>().getActivatedJointControllers().at(idx);
+ status.activeControlMode = activeJointCtrl ? activeJointCtrl->getControlMode()
+ : std::string{"!!JointController is nullptr!!"};
status.deviceName = controlDevice->getDeviceName();
- const auto requestedJointControllers = _module<ControlThreadDataBuffer>().copyRequestedJointControllers();
+ const auto requestedJointControllers =
+ _module<ControlThreadDataBuffer>().copyRequestedJointControllers();
ARMARX_CHECK_EXPRESSION(requestedJointControllers.at(idx));
status.requestedControlMode = requestedJointControllers.at(idx)->getControlMode();
- for (const auto& targ : _module<ControlThreadDataBuffer>().getSensorAndControlBuffer().control.at(idx))
+ for (const auto& targ :
+ _module<ControlThreadDataBuffer>().getSensorAndControlBuffer().control.at(idx))
{
- status.controlTargetValues[targ->getControlMode()] = targ->toVariants(_module<ControlThreadDataBuffer>().getSensorAndControlBuffer().sensorValuesTimestamp);
+ status.controlTargetValues[targ->getControlMode()] =
+ targ->toVariants(_module<ControlThreadDataBuffer>()
+ .getSensorAndControlBuffer()
+ .sensorValuesTimestamp);
}
status.timestampUSec = TimeUtil::GetTime(true).toMicroSeconds();
throwIfDevicesNotReady(__FUNCTION__);
return status;
}
- ControlDeviceStatus Devices::getControlDeviceStatus(const std::string& name, const Ice::Current&) const
+ ControlDeviceStatus
+ Devices::getControlDeviceStatus(const std::string& name, const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
if (!controlDevices.has(name))
{
std::stringstream ss;
ss << "getControlDeviceStatus: There is no ControlDevice '" << name
<< "'. There are these ControlDevices: " << controlDevices.keys();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
return getControlDeviceStatus(controlDevices.index(name));
}
- ControlDeviceStatusSeq Devices::getControlDeviceStatuses(const Ice::Current&) const
+ ControlDeviceStatusSeq
+ Devices::getControlDeviceStatuses(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (!areDevicesReady())
{
return {};
}
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
ControlDeviceStatusSeq r;
r.reserve(getNumberOfControlDevices());
for (auto idx : getIndices(controlDevices.values()))
@@ -193,19 +218,21 @@ namespace armarx::RobotUnitModule
return r;
}
- Ice::StringSeq Devices::getSensorDeviceNames(const Ice::Current&) const
+ Ice::StringSeq
+ Devices::getSensorDeviceNames(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ std::lock_guard<MutexType> guard{sensorDevicesMutex};
return sensorDevices.keys();
}
- SensorDeviceDescription Devices::getSensorDeviceDescription(std::size_t idx) const
+ SensorDeviceDescription
+ Devices::getSensorDeviceDescription(std::size_t idx) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ std::lock_guard<MutexType> guard{sensorDevicesMutex};
const SensorDevicePtr& sensorDevice = sensorDevices.at(idx);
SensorDeviceDescription data;
data.deviceName = sensorDevice->getDeviceName();
@@ -215,39 +242,47 @@ namespace armarx::RobotUnitModule
return data;
}
- SensorDeviceStatus Devices::getSensorDeviceStatus(std::size_t idx) const
+ SensorDeviceStatus
+ Devices::getSensorDeviceStatus(std::size_t idx) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ std::lock_guard<MutexType> guard{sensorDevicesMutex};
const SensorDevicePtr& sensorDevice = sensorDevices.at(idx);
SensorDeviceStatus status;
status.deviceName = sensorDevice->getDeviceName();
- status.sensorValue = _module<ControlThreadDataBuffer>().getSensorAndControlBuffer().sensors.at(idx)->toVariants(_module<ControlThreadDataBuffer>().getSensorAndControlBuffer().sensorValuesTimestamp);
+ status.sensorValue = _module<ControlThreadDataBuffer>()
+ .getSensorAndControlBuffer()
+ .sensors.at(idx)
+ ->toVariants(_module<ControlThreadDataBuffer>()
+ .getSensorAndControlBuffer()
+ .sensorValuesTimestamp);
status.timestampUSec = TimeUtil::GetTime(true).toMicroSeconds();
throwIfDevicesNotReady(__FUNCTION__);
return status;
}
- SensorDeviceDescription Devices::getSensorDeviceDescription(const std::string& name, const Ice::Current&) const
+ SensorDeviceDescription
+ Devices::getSensorDeviceDescription(const std::string& name, const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ std::lock_guard<MutexType> guard{sensorDevicesMutex};
if (!sensorDevices.has(name))
{
std::stringstream ss;
ss << "getSensorDeviceDescription: There is no SensorDevice '" << name
<< "'. There are these SensorDevices: " << sensorDevices.keys();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
return getSensorDeviceDescription(sensorDevices.index(name));
}
- SensorDeviceDescriptionSeq Devices::getSensorDeviceDescriptions(const Ice::Current&) const
+ SensorDeviceDescriptionSeq
+ Devices::getSensorDeviceDescriptions(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ std::lock_guard<MutexType> guard{sensorDevicesMutex};
if (!areDevicesReady())
{
return {};
@@ -261,29 +296,31 @@ namespace armarx::RobotUnitModule
return r;
}
- SensorDeviceStatus Devices::getSensorDeviceStatus(const std::string& name, const Ice::Current&) const
+ SensorDeviceStatus
+ Devices::getSensorDeviceStatus(const std::string& name, const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ std::lock_guard<MutexType> guard{sensorDevicesMutex};
if (!sensorDevices.has(name))
{
std::stringstream ss;
ss << "getSensorDeviceStatus: There is no SensorDevice '" << name
<< "'. There are these SensorDevices: " << sensorDevices.keys();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
return getSensorDeviceStatus(sensorDevices.index(name));
}
- SensorDeviceStatusSeq Devices::getSensorDeviceStatuses(const Ice::Current&) const
+ SensorDeviceStatusSeq
+ Devices::getSensorDeviceStatuses(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (!areDevicesReady())
{
return {};
}
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ std::lock_guard<MutexType> guard{sensorDevicesMutex};
SensorDeviceStatusSeq r;
r.reserve(getNumberOfSensorDevices());
for (auto idx : getIndices(sensorDevices.values()))
@@ -294,20 +331,21 @@ namespace armarx::RobotUnitModule
return r;
}
- void Devices::addControlDevice(const ControlDevicePtr& cd)
+ void
+ Devices::addControlDevice(const ControlDevicePtr& cd)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_DEBUG << "ControlDevice " << &cd;
+ ARMARX_DEBUG << "ControlDevice " << &cd;
throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
{
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
//check it
if (!cd)
{
std::stringstream ss;
ss << "armarx::RobotUnit::addControlDevice: ControlDevice is nullptr";
ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
if (!cd->getJointEmergencyStopController())
{
@@ -315,7 +353,7 @@ namespace armarx::RobotUnitModule
ss << "armarx::RobotUnit::addControlDevice: ControlDevice " << cd->getDeviceName()
<< " has null JointEmergencyStopController (this is not allowed)";
ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
if (!cd->getJointStopMovementController())
{
@@ -323,10 +361,10 @@ namespace armarx::RobotUnitModule
ss << "armarx::RobotUnit::addControlDevice: ControlDevice " << cd->getDeviceName()
<< " has null getJointStopMovementController (this is not allowed)";
ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
//add it
- ARMARX_DEBUG << "Adding the ControlDevice " << cd->getDeviceName() << " " << &cd ;
+ ARMARX_DEBUG << "Adding the ControlDevice " << cd->getDeviceName() << " " << &cd;
controlDevices.add(cd->getDeviceName(), cd);
cd->owner = this;
ARMARX_INFO << "added ControlDevice " << cd->getDeviceName();
@@ -335,20 +373,21 @@ namespace armarx::RobotUnitModule
throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
}
- void Devices::addSensorDevice(const SensorDevicePtr& sd)
+ void
+ Devices::addSensorDevice(const SensorDevicePtr& sd)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_DEBUG << "SensorDevice " << &sd;
+ ARMARX_DEBUG << "SensorDevice " << &sd;
throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
{
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ std::lock_guard<MutexType> guard{sensorDevicesMutex};
//check it
if (!sd)
{
std::stringstream ss;
ss << "armarx::RobotUnit::addSensorDevice: SensorDevice is nullptr";
ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
if (!sd->getSensorValue())
{
@@ -356,76 +395,79 @@ namespace armarx::RobotUnitModule
ss << "armarx::RobotUnit::addSensorDevice: SensorDevice " << sd->getDeviceName()
<< " has null SensorValue (this is not allowed)";
ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
//add it
if (sd->getDeviceName() == rtThreadTimingsSensorDeviceName)
{
- ARMARX_DEBUG << "Device is the " << rtThreadTimingsSensorDeviceName;
+ ARMARX_DEBUG << "Device is the " << rtThreadTimingsSensorDeviceName;
if (!std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd))
{
- throw InvalidArgumentException
- {
+ throw InvalidArgumentException{
"You tried to add a SensorDevice with the name " + sd->getDeviceName() +
- " which does not derive from RTThreadTimingsSensorDevice. (Don't do this)"
- };
+ " which does not derive from RTThreadTimingsSensorDevice. (Don't do this)"};
}
//this checks if we already added such a device (do this before setting timingSensorDevice)
- ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd ;
+ ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd;
sensorDevices.add(sd->getDeviceName(), sd);
sd->owner = this;
- rtThreadTimingsSensorDevice = std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd);
+ rtThreadTimingsSensorDevice =
+ std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd);
}
- else if(sd->getDeviceName() == GlobalRobotLocalizationSensorDevice::DeviceName())
+ else if (sd->getDeviceName() == GlobalRobotLocalizationSensorDevice::DeviceName())
{
- ARMARX_DEBUG << "Device is the " << sd->getDeviceName();
+ ARMARX_DEBUG << "Device is the " << sd->getDeviceName();
if (!std::dynamic_pointer_cast<GlobalRobotLocalizationSensorDevice>(sd))
{
- throw InvalidArgumentException
- {
+ throw InvalidArgumentException{
"You tried to add a SensorDevice with the name " + sd->getDeviceName() +
- " which does not derive from GlobalRobotLocalizationSensorDevice. (Don't do this)"
- };
+ " which does not derive from GlobalRobotLocalizationSensorDevice. (Don't "
+ "do this)"};
}
//this checks if we already added such a device (do this before setting timingSensorDevice)
- ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd ;
+ ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd;
sensorDevices.add(sd->getDeviceName(), sd);
sd->owner = this;
- globalRobotLocalizationSensorDevice = sd; //std::dynamic_pointer_cast<GlobalRobotLocalizationSensorDevice>(sd);
+ globalRobotLocalizationSensorDevice =
+ sd; //std::dynamic_pointer_cast<GlobalRobotLocalizationSensorDevice>(sd);
}
else
{
- ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd ;
+ ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd;
sensorDevices.add(sd->getDeviceName(), sd);
sd->owner = this;
}
}
- ARMARX_INFO << "added SensorDevice " << sd->getDeviceName() << " (valuetype = " << sd->getSensorValueType() << ")";
+ ARMARX_INFO << "added SensorDevice " << sd->getDeviceName()
+ << " (valuetype = " << sd->getSensorValueType() << ")";
throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
}
- RTThreadTimingsSensorDevicePtr Devices::createRTThreadTimingSensorDevice() const
+ RTThreadTimingsSensorDevicePtr
+ Devices::createRTThreadTimingSensorDevice() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return std::make_shared<RTThreadTimingsSensorDeviceImpl<>>(rtThreadTimingsSensorDeviceName);
}
- void Devices::_postFinishRunning()
+ void
+ Devices::_postFinishRunning()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<MutexType> guardS {sensorDevicesMutex};
- std::lock_guard<MutexType> guardC {controlDevicesMutex};
+ std::lock_guard<MutexType> guardS{sensorDevicesMutex};
+ std::lock_guard<MutexType> guardC{controlDevicesMutex};
controlDevicesConstPtr.clear();
sensorDevicesConstPtr.clear();
sensorDevices.clear();
controlDevices.clear();
}
- std::vector<JointController*> Devices::getStopMovementJointControllers() const
+ std::vector<JointController*>
+ Devices::getStopMovementJointControllers() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
std::vector<JointController*> controllers;
controllers.reserve(controlDevices.values().size());
for (const ControlDevicePtr& dev : controlDevices.values())
@@ -439,11 +481,12 @@ namespace armarx::RobotUnitModule
return controllers;
}
- std::vector<JointController*> Devices::getEmergencyStopJointControllers() const
+ std::vector<JointController*>
+ Devices::getEmergencyStopJointControllers() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
std::vector<JointController*> controllers;
controllers.reserve(controlDevices.values().size());
for (const ControlDevicePtr& dev : controlDevices.values())
@@ -457,11 +500,12 @@ namespace armarx::RobotUnitModule
return controllers;
}
- void Devices::_preFinishDeviceInitialization()
+ void
+ Devices::_preFinishDeviceInitialization()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<MutexType> guardS {sensorDevicesMutex};
- std::lock_guard<MutexType> guardC {controlDevicesMutex};
+ std::lock_guard<MutexType> guardS{sensorDevicesMutex};
+ std::lock_guard<MutexType> guardC{controlDevicesMutex};
if (!sensorDevices.has(rtThreadTimingsSensorDeviceName))
{
addSensorDevice(createRTThreadTimingSensorDevice());
@@ -473,12 +517,13 @@ namespace armarx::RobotUnitModule
addSensorDevice(std::make_shared<GlobalRobotLocalizationSensorDevice>());
}
- void Devices::_postFinishDeviceInitialization()
+ void
+ Devices::_postFinishDeviceInitialization()
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<MutexType> guardS {sensorDevicesMutex};
- std::lock_guard<MutexType> guardC {controlDevicesMutex};
+ std::lock_guard<MutexType> guardS{sensorDevicesMutex};
+ std::lock_guard<MutexType> guardC{controlDevicesMutex};
ARMARX_DEBUG << "checking " << controlDevices.size() << " ControlDevices:";
{
ARMARX_TRACE;
@@ -491,21 +536,22 @@ namespace armarx::RobotUnitModule
std::stringstream s;
s << "ControlDevice " << controlDevice->getDeviceName()
<< " has no JointController with ControlMode " << ControlModes::EmergencyStop
- << " (to fix this, add a JointController with this ControlMode to the ControlDevice).\nAvailable controllers: "
- << controlDevice->getControlModes();
+ << " (to fix this, add a JointController with this ControlMode to the "
+ "ControlDevice).\nAvailable controllers: "
+ << controlDevice->getControlModes();
ARMARX_ERROR << "--------" << s.str();
- throw LogicError {s.str()};
-
+ throw LogicError{s.str()};
}
if (!controlDevice->hasJointController(ControlModes::StopMovement))
{
std::stringstream s;
s << "ControlDevice " << controlDevice->getDeviceName()
<< " has no JointController with ControlMode \"" << ControlModes::StopMovement
- << "\" (to fix this, add a JointController with this ControlMode to the ControlDevice) \nAvailable controllers: "
+ << "\" (to fix this, add a JointController with this ControlMode to the "
+ "ControlDevice) \nAvailable controllers: "
<< controlDevice->getControlModes();
ARMARX_ERROR << "--------" << s.str();
- throw LogicError {s.str()};
+ throw LogicError{s.str()};
}
}
}
@@ -519,9 +565,10 @@ namespace armarx::RobotUnitModule
if (!sensorDevice->getSensorValue())
{
std::stringstream s;
- s << "SensorDevice " << sensorDevice->getSensorValue() << " has null SensorValue";
+ s << "SensorDevice " << sensorDevice->getSensorValue()
+ << " has null SensorValue";
ARMARX_ERROR << "--------" << s.str();
- throw LogicError {s.str()};
+ throw LogicError{s.str()};
}
ARMARX_CHECK_EXPRESSION(sensorDevice);
}
@@ -611,8 +658,9 @@ namespace armarx::RobotUnitModule
}
if (!ctrlModeGroups.groupsMerged.empty())
{
- ARMARX_DEBUG << "Checking control modes for ControlDeviceHardwareControlModeGroups ("
- << ctrlModeGroups.groups.size() << ")";
+ ARMARX_DEBUG
+ << "Checking control modes for ControlDeviceHardwareControlModeGroups ("
+ << ctrlModeGroups.groups.size() << ")";
ctrlModeGroups.deviceIndices.resize(ctrlModeGroups.groups.size());
//iterate over groups
for (std::size_t groupIdx = 0; groupIdx < ctrlModeGroups.groups.size(); ++groupIdx)
@@ -622,38 +670,43 @@ namespace armarx::RobotUnitModule
ARMARX_CHECK_EXPRESSION(!group.empty());
ARMARX_DEBUG << "----Group " << groupIdx << " size: " << group.size();
//gets a map of ControlMode->HardwareControlMode for the given device
- const auto getControlModeToHWControlMode = [&](const std::string & devname)
+ const auto getControlModeToHWControlMode = [&](const std::string& devname)
{
std::map<std::string, std::string> controlModeToHWControlMode;
const ControlDevicePtr& cd = controlDevices.at(devname);
for (const auto& jointCtrl : cd->getJointControllers())
{
- controlModeToHWControlMode[jointCtrl->getControlMode()] = jointCtrl->getHardwareControlMode();
+ controlModeToHWControlMode[jointCtrl->getControlMode()] =
+ jointCtrl->getHardwareControlMode();
}
return controlModeToHWControlMode;
};
//get modes of first dev
- const auto controlModeToHWControlMode = getControlModeToHWControlMode(*group.begin());
+ const auto controlModeToHWControlMode =
+ getControlModeToHWControlMode(*group.begin());
//check other devs
for (const auto& devname : group)
{
- ARMARX_CHECK_EXPRESSION(
- controlDevices.has(devname)) <<
- "The ControlDeviceHardwareControlModeGroups property contains device names not existent in the robot: "
- << devname << "\navailable:\n" << controlDevices.keys();
+ ARMARX_CHECK_EXPRESSION(controlDevices.has(devname))
+ << "The ControlDeviceHardwareControlModeGroups property contains "
+ "device names not existent in the robot: "
+ << devname << "\navailable:\n"
+ << controlDevices.keys();
//Assert all devices in a group have the same control modes with the same hw controle modes
- const auto controlModeToHWControlModeForDevice = getControlModeToHWControlMode(devname);
- ARMARX_CHECK_EXPRESSION(
- controlModeToHWControlModeForDevice == controlModeToHWControlMode) <<
- "Error for control modes of device '" << devname << "'\n"
- << "it has the modes: " << controlModeToHWControlModeForDevice
- << "\n but should have the modes: " << controlModeToHWControlMode;
+ const auto controlModeToHWControlModeForDevice =
+ getControlModeToHWControlMode(devname);
+ ARMARX_CHECK_EXPRESSION(controlModeToHWControlModeForDevice ==
+ controlModeToHWControlMode)
+ << "Error for control modes of device '" << devname << "'\n"
+ << "it has the modes: " << controlModeToHWControlModeForDevice
+ << "\n but should have the modes: " << controlModeToHWControlMode;
//insert the device index into the device indices
const auto devIdx = controlDevices.index(devname);
ctrlModeGroups.deviceIndices.at(groupIdx).emplace_back(devIdx);
//insert the group index into the group indices (+ check the current group index is the sentinel
ARMARX_CHECK_EXPRESSION(ctrlModeGroups.groupIndices.size() > devIdx);
- ARMARX_CHECK_EXPRESSION(ctrlModeGroups.groupIndices.at(devIdx) == IndexSentinel());
+ ARMARX_CHECK_EXPRESSION(ctrlModeGroups.groupIndices.at(devIdx) ==
+ IndexSentinel());
ctrlModeGroups.groupIndices.at(devIdx) = groupIdx;
ARMARX_DEBUG << "------- " << devname;
}
@@ -684,10 +737,10 @@ namespace armarx::RobotUnitModule
}
else
{
- ARMARX_WARNING << "SensorValue for SensorDevice "
- << name << " is of type "
- << dev->getSensorValueType()
- << " which does not derive SensorValue1DoFActuatorPosition";
+ ARMARX_WARNING
+ << "SensorValue for SensorDevice " << name << " is of type "
+ << dev->getSensorValueType()
+ << " which does not derive SensorValue1DoFActuatorPosition";
}
}
else
@@ -702,19 +755,22 @@ namespace armarx::RobotUnitModule
ARMARX_VERBOSE << "SensorDevices:\n" << sensorDevices.keys();
}
- void Devices::_preOnInitRobotUnit()
+ void
+ Devices::_preOnInitRobotUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
//ControlDeviceHardwareControlModeGroups
- const std::string controlDeviceHardwareControlModeGroupsStr = getProperty<std::string>("ControlDevices_HardwareControlModeGroups").getValue();
+ const std::string controlDeviceHardwareControlModeGroupsStr =
+ getProperty<std::string>("ControlDevices_HardwareControlModeGroups").getValue();
if (!controlDeviceHardwareControlModeGroupsStr.empty())
{
- const auto numGroups = std::count(
- controlDeviceHardwareControlModeGroupsStr.begin(),
- controlDeviceHardwareControlModeGroupsStr.end(), ';'
- ) + 1;
+ const auto numGroups = std::count(controlDeviceHardwareControlModeGroupsStr.begin(),
+ controlDeviceHardwareControlModeGroupsStr.end(),
+ ';') +
+ 1;
ctrlModeGroups.groups.reserve(numGroups);
- std::vector<std::string> strGroups = Split(controlDeviceHardwareControlModeGroupsStr, ";");
+ std::vector<std::string> strGroups =
+ Split(controlDeviceHardwareControlModeGroupsStr, ";");
for (const auto& gstr : strGroups)
{
bool trimDeviceNames = true;
@@ -722,19 +778,19 @@ namespace armarx::RobotUnitModule
std::set<std::string> group;
for (auto& device : strElems)
{
- ARMARX_CHECK_EXPRESSION(
- !device.empty()) <<
- "The ControlDeviceHardwareControlModeGroups property contains empty device names";
- ARMARX_CHECK_EXPRESSION(
- !ctrlModeGroups.groupsMerged.count(device)) <<
- "The ControlDeviceHardwareControlModeGroups property contains duplicate device names: " << device;
+ ARMARX_CHECK_EXPRESSION(!device.empty())
+ << "The ControlDeviceHardwareControlModeGroups property contains empty "
+ "device names";
+ ARMARX_CHECK_EXPRESSION(!ctrlModeGroups.groupsMerged.count(device))
+ << "The ControlDeviceHardwareControlModeGroups property contains duplicate "
+ "device names: "
+ << device;
ctrlModeGroups.groupsMerged.emplace(device);
group.emplace(std::move(device));
}
if (!group.empty())
{
- ARMARX_DEBUG << "adding device group:\n"
- << ARMARX_STREAM_PRINTER
+ ARMARX_DEBUG << "adding device group:\n" << ARMARX_STREAM_PRINTER
{
for (const auto& elem : group)
{
@@ -746,4 +802,4 @@ namespace armarx::RobotUnitModule
}
}
}
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h
index 9a32f682c421ca571786037c542d24195f915a35..0c7caccb3cf6fa840928c6a5907f483e21690d5a 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h
@@ -24,35 +24,37 @@
#include <mutex>
-#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include "RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h"
-#include "RobotUnitModuleBase.h"
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include "../Devices/ControlDevice.h"
-#include "../Devices/SensorDevice.h"
#include "../Devices/RTThreadTimingsSensorDevice.h"
-
+#include "../Devices/SensorDevice.h"
#include "../SensorValues/SensorValue1DoFActuator.h"
+#include "RobotUnitModuleBase.h"
namespace armarx::RobotUnitModule
{
- class DevicesPropertyDefinitions: public ModuleBasePropertyDefinitions
+ class DevicesPropertyDefinitions : public ModuleBasePropertyDefinitions
{
public:
- DevicesPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
+ DevicesPropertyDefinitions(std::string prefix) : ModuleBasePropertyDefinitions(prefix)
{
defineOptionalProperty<std::string>(
- "ControlDevices_HardwareControlModeGroups", "",
+ "ControlDevices_HardwareControlModeGroups",
+ "",
"Groups of control devices for which the same hardware control mode has to be set. "
- "For a control mode, the JointControllers of all devices in a group must have the identical hardware control mode. "
+ "For a control mode, the JointControllers of all devices in a group must have the "
+ "identical hardware control mode. "
"The kinematic unit will change the control mode of all joints in a group,"
" if their current hw control mode does not match the new hw control mode. "
- "This can be used if the robot has a multi DOF joint which is represented by multiple 1DOF control devices "
+ "This can be used if the robot has a multi DOF joint which is represented by "
+ "multiple 1DOF control devices "
"(in such a case all of these 1DOF joints should have the same hw control mode, "
"except he multi DOF joint can handle different control modes at once). "
"Syntax: semicolon separated groups , each group is CSV of joints "
- "e.g. j1,j2;j3,j4,j5 -> Group 1 = j1+j2 Group 2 = j3+j4+j5; No joint may be in two groups!"
- );
+ "e.g. j1,j2;j3,j4,j5 -> Group 1 = j1+j2 Group 2 = j3+j4+j5; No joint may be in two "
+ "groups!");
}
};
@@ -65,16 +67,20 @@ namespace armarx::RobotUnitModule
class Devices : virtual public ModuleBase, virtual public RobotUnitDevicesInterface
{
friend class ModuleBase;
+
public:
struct ControlDeviceHardwareControlModeGroups;
+
/**
* @brief Returns the singleton instance of this class
* @return The singleton instance of this class
*/
- static Devices& Instance()
+ static Devices&
+ Instance()
{
return ModuleBase::Instance<Devices>();
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -95,7 +101,8 @@ namespace armarx::RobotUnitModule
* @brief Returns the names of all \ref ControlDevice "ControlDevices" for the robot.
* @return The names of all \ref ControlDevice "ControlDevices" for the robot.
*/
- Ice::StringSeq getControlDeviceNames(const Ice::Current& = Ice::emptyCurrent) const override;
+ Ice::StringSeq
+ getControlDeviceNames(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Return the \ref ControlDeviceDescription for the given \ref ControlDevice
@@ -104,14 +111,17 @@ namespace armarx::RobotUnitModule
* @see ControlDeviceDescription
* @see getControlDeviceDescriptions
*/
- ControlDeviceDescription getControlDeviceDescription(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ ControlDeviceDescription
+ getControlDeviceDescription(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Return the \ref ControlDeviceDescription "ControlDeviceDescriptions" for all \ref ControlDevice "ControlDevices"
* @return The \ref ControlDeviceDescription "ControlDeviceDescriptions"
* @see ControlDeviceDescription
* @see getControlDeviceDescription
*/
- ControlDeviceDescriptionSeq getControlDeviceDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
+ ControlDeviceDescriptionSeq
+ getControlDeviceDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Return the \ref ControlDeviceStatus for the given \ref ControlDevice
@@ -120,14 +130,17 @@ namespace armarx::RobotUnitModule
* @see ControlDeviceStatus
* @see getControlDeviceStatuses
*/
- ControlDeviceStatus getControlDeviceStatus(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ ControlDeviceStatus
+ getControlDeviceStatus(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Return the \ref ControlDeviceStatus "ControlDeviceStatuses" for all \ref ControlDevice "ControlDevices"
* @return The \ref ControlDeviceStatus "ControlDeviceStatuses"
* @see ControlDeviceStatus
* @see getControlDeviceStatus
*/
- ControlDeviceStatusSeq getControlDeviceStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
+ ControlDeviceStatusSeq
+ getControlDeviceStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the names of all \ref SensorDevice "SensorDevices" for the robot.
@@ -142,14 +155,17 @@ namespace armarx::RobotUnitModule
* @see SensorDeviceDescription
* @see getSensorDeviceDescriptions
*/
- SensorDeviceDescription getSensorDeviceDescription(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ SensorDeviceDescription
+ getSensorDeviceDescription(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Return the \ref SensorDeviceDescription "SensorDeviceDescriptions" for all \ref SensorDevice "SensorDevices"
* @return The \ref SensorDeviceDescription "SensorDeviceDescriptions"
* @see SensorDeviceDescription
* @see getSensorDeviceDescription
*/
- SensorDeviceDescriptionSeq getSensorDeviceDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
+ SensorDeviceDescriptionSeq
+ getSensorDeviceDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Return the \ref SensorDeviceStatus for the given \ref SensorDevice
@@ -158,14 +174,17 @@ namespace armarx::RobotUnitModule
* @see SensorDeviceStatus
* @see getSensorDeviceStatuses
*/
- SensorDeviceStatus getSensorDeviceStatus(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ SensorDeviceStatus
+ getSensorDeviceStatus(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Return the \ref SensorDeviceStatus "SensorDeviceStatuses" for all \ref SensorDevice "SensorDevices"
* @return The \ref SensorDeviceStatus "SensorDeviceStatuses"
* @see SensorDeviceStatus
* @see getSensorDeviceStatus
*/
- SensorDeviceStatusSeq getSensorDeviceStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
+ SensorDeviceStatusSeq
+ getSensorDeviceStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
// //////////////////////////////////////////////////////////////////////////////////////// //
// /////////////////////////////////// Module interface /////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -224,11 +243,14 @@ namespace armarx::RobotUnitModule
* @param robot The VirtualRobot to update
* @param sensors The \ref SensorValue "SensorValues"
*/
- template<class PtrT>
- void updateVirtualRobotFromSensorValues(const VirtualRobot::RobotPtr& robot, const std::vector<PtrT>& sensors) const
+ template <class PtrT>
+ void
+ updateVirtualRobotFromSensorValues(const VirtualRobot::RobotPtr& robot,
+ const std::vector<PtrT>& sensors) const
{
updateVirtualRobotFromSensorValues(robot, robot->getRobotNodes(), sensors);
}
+
/**
* @brief Updates the given VirtualRobot with the given \ref SensorValue "SensorValues"
*
@@ -237,8 +259,11 @@ namespace armarx::RobotUnitModule
* @param nodes The VirtualRobot's RobotNodes
* @param sensors The \ref SensorValue "SensorValues"
*/
- template<class PtrT>
- void updateVirtualRobotFromSensorValues(const VirtualRobot::RobotPtr& robot, const std::vector<VirtualRobot::RobotNodePtr>& nodes, const std::vector<PtrT>& sensors) const
+ template <class PtrT>
+ void
+ updateVirtualRobotFromSensorValues(const VirtualRobot::RobotPtr& robot,
+ const std::vector<VirtualRobot::RobotNodePtr>& nodes,
+ const std::vector<PtrT>& sensors) const
{
ARMARX_CHECK_NOT_NULL(robot);
@@ -246,15 +271,18 @@ namespace armarx::RobotUnitModule
{
const SensorValueBase* sv = sensors.at(m.idxSens);
ARMARX_CHECK_NOT_NULL(sv);
- const SensorValue1DoFActuatorPosition* svPos = sv->asA<SensorValue1DoFActuatorPosition>();
+ const SensorValue1DoFActuatorPosition* svPos =
+ sv->asA<SensorValue1DoFActuatorPosition>();
ARMARX_CHECK_NOT_NULL(svPos);
nodes.at(m.idxRobot)->setJointValueNoUpdate(svPos->position);
}
- // update global pose
- if(globalRobotLocalizationSensorDevice != nullptr)
+ // update global pose
+ if (globalRobotLocalizationSensorDevice != nullptr)
{
- const auto sensorValue = globalRobotLocalizationSensorDevice->getSensorValue()->asA<GlobalRobotLocalizationSensorDevice::SensorValueType>();
+ const auto sensorValue =
+ globalRobotLocalizationSensorDevice->getSensorValue()
+ ->asA<GlobalRobotLocalizationSensorDevice::SensorValueType>();
const auto global_T_root = sensorValue->global_T_root;
robot->setGlobalPose(global_T_root, false);
}
@@ -268,7 +296,8 @@ namespace armarx::RobotUnitModule
* @return The groups of \ref ControlDevice "ControlDevices" required to be in the same
* hardware control mode.
*/
- const ControlDeviceHardwareControlModeGroups& getControlModeGroups() const
+ const ControlDeviceHardwareControlModeGroups&
+ getControlModeGroups() const
{
return ctrlModeGroups;
}
@@ -291,19 +320,22 @@ namespace armarx::RobotUnitModule
* @param deviceName \ref ControlDevice's name.
* @return The \ref ControlDevice
*/
- ConstControlDevicePtr getControlDevice(const std::string& deviceName) const; /// TODO move to attorney for NJointControllerBase
+ ConstControlDevicePtr getControlDevice(
+ const std::string& deviceName) const; /// TODO move to attorney for NJointControllerBase
/**
* @brief Returns the \ref SensorDevice
* @param deviceName \ref SensorDevice's name.
* @return The \ref SensorDevice
*/
- ConstSensorDevicePtr getSensorDevice(const std::string& deviceName) const; /// TODO move to attorney for NJointControllerBase
+ ConstSensorDevicePtr getSensorDevice(
+ const std::string& deviceName) const; /// TODO move to attorney for NJointControllerBase
/**
* @brief Returns all \ref SensorDevice "SensorDevices"
* @return All \ref SensorDevice "SensorDevices"
*/
- const KeyValueVector<std::string, SensorDevicePtr>& getSensorDevices() const ///TODO remove from interface
+ const KeyValueVector<std::string, SensorDevicePtr>&
+ getSensorDevices() const ///TODO remove from interface
{
return sensorDevices;
}
@@ -312,7 +344,8 @@ namespace armarx::RobotUnitModule
* @brief Returns all \ref ControlDevice "ControlDevices"
* @return All \ref ControlDevice "ControlDevices"
*/
- const KeyValueVector<std::string, ControlDevicePtr>& getControlDevices() const ///TODO remove from interface
+ const KeyValueVector<std::string, ControlDevicePtr>&
+ getControlDevices() const ///TODO remove from interface
{
return controlDevices;
}
@@ -321,18 +354,22 @@ namespace armarx::RobotUnitModule
* @brief Returns const pointers to all \ref SensorDevice "SensorDevices"
* @return Const pointers to all \ref SensorDevice "SensorDevices"
*/
- const std::map<std::string, ConstSensorDevicePtr>& getSensorDevicesConstPtr() const ///TODO rename to getSensorDevices
+ const std::map<std::string, ConstSensorDevicePtr>&
+ getSensorDevicesConstPtr() const ///TODO rename to getSensorDevices
{
return sensorDevicesConstPtr;
}
+
/**
* @brief Returns const pointers to all \ref ControlDevice "ControlDevices"
* @return Const pointers to all \ref ControlDevice "ControlDevices"
*/
- const std::map<std::string, ConstControlDevicePtr>& getControlDevicesConstPtr() const ///TODO rename to getControlDevices
+ const std::map<std::string, ConstControlDevicePtr>&
+ getControlDevicesConstPtr() const ///TODO rename to getControlDevices
{
return controlDevicesConstPtr;
}
+
protected:
/**
* @brief Adds a \ref ControlDevice to the robot.
@@ -353,16 +390,19 @@ namespace armarx::RobotUnitModule
* @see rtGetRTThreadTimingsSensorDevice
*/
virtual RTThreadTimingsSensorDevicePtr createRTThreadTimingSensorDevice() const;
+
/**
* @brief Returns the \ref SensorDevice used to log timings in the \ref ControlThread.
* (This function is supposed to be used in the \ref ControlThread)
* @return The \ref SensorDevice used to log timings in the \ref ControlThread
* @see createRTThreadTimingSensorDevice
*/
- RTThreadTimingsSensorDevice& rtGetRTThreadTimingsSensorDevice()
+ RTThreadTimingsSensorDevice&
+ rtGetRTThreadTimingsSensorDevice()
{
return *rtThreadTimingsSensorDevice;
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// //////////////////////////////////// implementation //////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -391,12 +431,14 @@ namespace armarx::RobotUnitModule
/// @brief The \ref SensorDevice's index
std::size_t idxSens;
};
+
/// @brief Mapping from RobotNode index to \ref SensorDevice
std::vector<SimoxRobotSensorValueMapping> simoxRobotSensorValueMapping;
/// @brief Pointers to all \ref SensorValueBase "SensorValues"
std::vector<const SensorValueBase*> sensorValues;
/// @brief Pointers to all \ref ControlTargetBase "ControlTargets"
std::vector<std::vector<PropagateConst<ControlTargetBase*>>> controlTargets;
+
public:
/// @brief Holds data about a device group requiring the same hardware control mode. (explained \ref RobotUnitCtrlModeGroups "here")
struct ControlDeviceHardwareControlModeGroups
@@ -412,6 +454,7 @@ namespace armarx::RobotUnitModule
/// @brief contains the group index for each device (or the sentinel)
std::vector<std::size_t> groupIndices;
};
+
private:
//ctrl dev
/// @brief \ref ControlDevice "ControlDevices" added to this unit (data may only be added during and only be used after \ref State::InitializingDevices)
@@ -426,7 +469,7 @@ namespace armarx::RobotUnitModule
//sens dev
/// @brief \ref SensorDevice "SensorDevices" added to this unit (data may only be added during and only be used after \ref State::InitializingDevices)
- KeyValueVector<std::string, SensorDevicePtr > sensorDevices;
+ KeyValueVector<std::string, SensorDevicePtr> sensorDevices;
/// @brief const pointer to all SensorDevices (passed to GenerateConfigDescription of a NJointControllerBase)
std::map<std::string, ConstSensorDevicePtr> sensorDevicesConstPtr;
/// @brief Guards access to all \ref SensorDevice "SensorDevices"
@@ -464,4 +507,4 @@ namespace armarx::RobotUnitModule
*/
friend class DevicesAttorneyForNJointController;
};
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
index b74fdee0d664cbc610a985045db441136314daa3..4d3899860afa56a2d179504685ebb96d5bb483a1 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -540,7 +540,8 @@ namespace armarx::RobotUnitModule
const auto start = now();
doLogging();
const std::uint64_t ms =
- std::chrono::duration_cast<std::chrono::milliseconds>(now() - start).count();
+ std::chrono::duration_cast<std::chrono::milliseconds>(now() - start)
+ .count();
ARMARX_WARNING << deactivateSpam(10) << "logging thread took " << ms << " ms > "
<< rtLoggingTimestepMs
<< " ms (message printed every 10 seconds)";
@@ -1180,4 +1181,4 @@ Logging::DataStreamingEntry::send(const RobotUnitDataStreaming::ReceiverPrx& r,
updateCalls.erase(updateCalls.begin(), updateCalls.begin() + i);
}
}
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
index 7529c75fd0182aa30c7c64ff994df15127b29009..6238efd4dcdd7b3e574c95c7b00f394a513bb651 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
@@ -23,17 +23,17 @@
#pragma once
#include <atomic>
-#include <map>
-#include <vector>
#include <deque>
#include <fstream>
+#include <map>
+#include <vector>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-#include "RobotUnitModuleBase.h"
#include "../util/ControlThreadOutputBuffer.h"
+#include "RobotUnitModuleBase.h"
namespace armarx::RobotUnitModule::details
{
@@ -42,54 +42,67 @@ namespace armarx::RobotUnitModule::details
namespace armarx::RobotUnitModule
{
- class LoggingPropertyDefinitions: public ModuleBasePropertyDefinitions
+ class LoggingPropertyDefinitions : public ModuleBasePropertyDefinitions
{
public:
- LoggingPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
+ LoggingPropertyDefinitions(std::string prefix) : ModuleBasePropertyDefinitions(prefix)
{
defineOptionalProperty<std::size_t>(
- "RTLogging_PeriodMs", 10,
+ "RTLogging_PeriodMs",
+ 10,
"Period of the rt-logging thread in milliseconds. "
"A high period can cause spikes in disk activity if data is logged to disk. "
"A low period causes more computation overhead and memory consumption. "
"Must not be 0.");
- defineOptionalProperty<std::string>(
- "RTLogging_DefaultLog", "",
- "If rt logging is active and a file path is given, all data is logged to this file.");
+ defineOptionalProperty<std::string>("RTLogging_DefaultLog",
+ "",
+ "If rt logging is active and a file path is given, "
+ "all data is logged to this file.");
defineOptionalProperty<std::size_t>(
- "RTLogging_MessageNumber", 1000,
+ "RTLogging_MessageNumber",
+ 1000,
"Number of messages that can be logged in the control thread");
defineOptionalProperty<std::size_t>(
- "RTLogging_MessageBufferSize", 1024 * 1024,
+ "RTLogging_MessageBufferSize",
+ 1024 * 1024,
"Number of bytes that can be logged in the control thread");
defineOptionalProperty<std::size_t>(
- "RTLogging_MaxMessageNumber", 16000,
+ "RTLogging_MaxMessageNumber",
+ 16000,
"Max number of messages that can be logged in the control thread");
defineOptionalProperty<std::size_t>(
- "RTLogging_MaxMessageBufferSize", 16 * 1024 * 1024,
+ "RTLogging_MaxMessageBufferSize",
+ 16 * 1024 * 1024,
"Max number of bytes that can be logged in the control thread");
defineOptionalProperty<bool>(
- "RTLogging_EnableBacklog", true,
+ "RTLogging_EnableBacklog",
+ true,
"Enable/Disable the backlog (SensorValues, ControlTargets, Messages) that is kept"
"and can be dumped in case of an error.");
+ defineOptionalProperty<std::size_t>("RTLogging_KeepIterationsForMs",
+ 60 * 1000,
+ "All logging data (SensorValues, ControlTargets, "
+ "Messages) is kept for this duration "
+ "and can be dumped in case of an error.");
defineOptionalProperty<std::size_t>(
- "RTLogging_KeepIterationsForMs", 60 * 1000,
- "All logging data (SensorValues, ControlTargets, Messages) is kept for this duration "
- "and can be dumped in case of an error.");
- defineOptionalProperty<std::size_t>(
- "RTLogging_MaxBacklogSize", 5000,
+ "RTLogging_MaxBacklogSize",
+ 5000,
"Maximum size of the backlog (SensorValues, ControlTargets, Messages) that is kept"
"and can be dumped in case of an error.");
defineOptionalProperty<std::size_t>(
- "RTLogging_StreamingDataMaxClientConnectionFailures", 10,
- "If sending data to a client fails this often, the client is removed from the list");
-
- defineOptionalProperty<bool>("RTLogging_LogAllMessages", true,
- "If true, logs all messages from the control thread. If false, only the newest message will be logged");
+ "RTLogging_StreamingDataMaxClientConnectionFailures",
+ 10,
+ "If sending data to a client fails this often, the client is removed from the "
+ "list");
+
+ defineOptionalProperty<bool>("RTLogging_LogAllMessages",
+ true,
+ "If true, logs all messages from the control thread. If "
+ "false, only the newest message will be logged");
}
};
@@ -106,15 +119,18 @@ namespace armarx::RobotUnitModule
class Logging : virtual public ModuleBase, virtual public RobotUnitLoggingInterface
{
friend class ModuleBase;
+
public:
/**
* @brief Returns the singleton instance of this class
* @return The singleton instance of this class
*/
- static Logging& Instance()
+ static Logging&
+ Instance()
{
return ModuleBase::Instance<Logging>();
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -137,7 +153,10 @@ namespace armarx::RobotUnitModule
* @param loggingNames The data fields to log.
* @return A handle to the log. If it's last copy is deleted, logging is stopped.
*/
- SimpleRemoteReferenceCounterBasePtr startRtLogging(const std::string& formatString, const Ice::StringSeq& loggingNames, const Ice::Current& = Ice::emptyCurrent) override;
+ SimpleRemoteReferenceCounterBasePtr
+ startRtLogging(const std::string& formatString,
+ const Ice::StringSeq& loggingNames,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Starts logging to a CSV file
* @param formatString The file to log to.
@@ -145,19 +164,25 @@ namespace armarx::RobotUnitModule
* If value is empty, key is used as heading.
* @return A handle to the log. If it's last copy is deleted, logging is stopped.
*/
- SimpleRemoteReferenceCounterBasePtr startRtLoggingWithAliasNames(const std::string& formatString, const StringStringDictionary& aliasNames, const Ice::Current& = Ice::emptyCurrent) override;
+ SimpleRemoteReferenceCounterBasePtr
+ startRtLoggingWithAliasNames(const std::string& formatString,
+ const StringStringDictionary& aliasNames,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Stops logging to the given log.
* @param token The log to close.
*/
- void stopRtLogging(const armarx::SimpleRemoteReferenceCounterBasePtr& token, const Ice::Current& = Ice::emptyCurrent) override;
+ void stopRtLogging(const armarx::SimpleRemoteReferenceCounterBasePtr& token,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Adds a string to the log (it is added in a special column).
* @param token The log.
* @param marker The string to add.
*/
- void addMarkerToRtLog(const SimpleRemoteReferenceCounterBasePtr& token, const std::string& marker, const Ice::Current& = Ice::emptyCurrent) override;
+ void addMarkerToRtLog(const SimpleRemoteReferenceCounterBasePtr& token,
+ const std::string& marker,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Returns the names of all loggable data fields.
@@ -169,16 +194,16 @@ namespace armarx::RobotUnitModule
* This helps debugging.
* @param formatString The file.
*/
- void writeRecentIterationsToFile(const std::string& formatString, const Ice::Current& = Ice::emptyCurrent) const override;
+ void writeRecentIterationsToFile(const std::string& formatString,
+ const Ice::Current& = Ice::emptyCurrent) const override;
- RobotUnitDataStreaming::DataStreamingDescription startDataStreaming(
- const RobotUnitDataStreaming::ReceiverPrx& receiver,
- const RobotUnitDataStreaming::Config& config,
- const Ice::Current& = Ice::emptyCurrent) override;
+ RobotUnitDataStreaming::DataStreamingDescription
+ startDataStreaming(const RobotUnitDataStreaming::ReceiverPrx& receiver,
+ const RobotUnitDataStreaming::Config& config,
+ const Ice::Current& = Ice::emptyCurrent) override;
- void stopDataStreaming(
- const RobotUnitDataStreaming::ReceiverPrx& receiver,
- const Ice::Current& = Ice::emptyCurrent) override;
+ void stopDataStreaming(const RobotUnitDataStreaming::ReceiverPrx& receiver,
+ const Ice::Current& = Ice::emptyCurrent) override;
// //////////////////////////////////////////////////////////////////////////////////////// //
// //////////////////////////////////// implementation //////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -188,7 +213,8 @@ namespace armarx::RobotUnitModule
void doLogging(details::DoLoggingDurations& durations,
const IceUtil::Time& now,
const ControlThreadOutputBuffer::Entry& data,
- std::size_t i, std::size_t num);
+ std::size_t i,
+ std::size_t num);
static bool MatchName(const std::string& pattern, const std::string& name);
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -199,7 +225,7 @@ namespace armarx::RobotUnitModule
struct CSVLoggingEntry
{
/// @brief Whether logging should be stopped.
- std::atomic_bool stopLogging {false};
+ std::atomic_bool stopLogging{false};
/// @brief Bools whether a control device field should be logged.
std::vector<std::vector<std::vector<char>>> loggedControlDeviceValues;
/// @brief Bools whether a sensor device field should be logged.
@@ -225,48 +251,47 @@ namespace armarx::RobotUnitModule
Double,
Skipped
};
+
struct OutVal
{
std::size_t idx;
- ValueT value = ValueT::Skipped;
+ ValueT value = ValueT::Skipped;
};
- bool stopStreaming = false;
+ bool stopStreaming = false;
- std::size_t numBools = 0;
- std::size_t numBytes = 0;
- std::size_t numShorts = 0;
- std::size_t numInts = 0;
- std::size_t numLongs = 0;
- std::size_t numFloats = 0;
+ std::size_t numBools = 0;
+ std::size_t numBytes = 0;
+ std::size_t numShorts = 0;
+ std::size_t numInts = 0;
+ std::size_t numLongs = 0;
+ std::size_t numFloats = 0;
std::size_t numDoubles = 0;
- bool onlyNewestFrame = false;
+ bool onlyNewestFrame = false;
std::size_t connectionFailures = 0;
std::size_t rtStreamMaxClientErrors = 0;
- std::vector<std::vector<OutVal>> sensDevs;
+ std::vector<std::vector<OutVal>> sensDevs;
std::vector<std::vector<std::vector<OutVal>>> ctrlDevs;
- RobotUnitDataStreaming::TimeStepSeq result;
- std::deque<RobotUnitDataStreaming::TimeStep> entryBuffer;
- std::deque<Ice::AsyncResultPtr> updateCalls;
+ RobotUnitDataStreaming::TimeStepSeq result;
+ std::deque<RobotUnitDataStreaming::TimeStep> entryBuffer;
+ std::deque<Ice::AsyncResultPtr> updateCalls;
void clearResult();
RobotUnitDataStreaming::TimeStep getResultElement();
RobotUnitDataStreaming::TimeStep allocateResultElement() const;
-
void processHeader(const ControlThreadOutputBuffer::Entry& e);
void processCtrl(const ControlTargetBase& val,
std::size_t didx,
std::size_t vidx,
std::size_t fidx);
- void processSens(const SensorValueBase& val,
- std::size_t didx,
- std::size_t fidx);
+ void processSens(const SensorValueBase& val, std::size_t didx, std::size_t fidx);
void send(const RobotUnitDataStreaming::ReceiverPrx& r, uint64_t msgId);
};
+
std::map<RobotUnitDataStreaming::ReceiverPrx, DataStreamingEntry> rtDataStreamingEntry;
std::uint64_t rtDataStreamingMsgID = 0;
@@ -278,9 +303,9 @@ namespace armarx::RobotUnitModule
/// @brief Handle for the default log. (This log can be enabled via a property and logs everything)
SimpleRemoteReferenceCounterBasePtr defaultLogHandle;
/// @brief The stored backlog (it is dumped when \ref writeRecentIterationsToFile is called)
- std::deque< ::armarx::detail::ControlThreadOutputBufferEntry > backlog;
+ std::deque<::armarx::detail::ControlThreadOutputBufferEntry> backlog;
/// @brief The control threads unix thread id
- Ice::Int controlThreadId {0};
+ Ice::Int controlThreadId{0};
/// @brief Mutex protecting the data structures of this class
mutable std::mutex rtLoggingMutex;
@@ -288,27 +313,28 @@ namespace armarx::RobotUnitModule
{
struct FieldMetaData
{
- std::string name;
+ std::string name;
const std::type_info* type;
};
+
std::vector<FieldMetaData> fields;
};
/// @brief Data field info for all \ref ControlTargetBase "ControlTargets" (dex x jctrl)
std::vector<std::vector<ValueMetaData>> controlDeviceValueMetaData;
/// @brief Data field info for all \ref SensorValueBase "SensorValues" (dev)
- std::vector< ValueMetaData > sensorDeviceValueMetaData;
+ std::vector<ValueMetaData> sensorDeviceValueMetaData;
/// @brief The initial size of the Message entry buffer
- std::size_t messageBufferSize {0};
+ std::size_t messageBufferSize{0};
/// @brief The initial number of Message entries
- std::size_t messageBufferNumberEntries {0};
+ std::size_t messageBufferNumberEntries{0};
/// @brief The maximal size of the Message entry buffer
- std::size_t messageBufferMaxSize {0};
+ std::size_t messageBufferMaxSize{0};
/// @brief The maximal number of Message entries
- std::size_t messageBufferMaxNumberEntries {0};
+ std::size_t messageBufferMaxNumberEntries{0};
/// @brief The logging thread's period
- std::size_t rtLoggingTimestepMs {0};
+ std::size_t rtLoggingTimestepMs{0};
/// @brief The time an entry should remain in the backlog.
IceUtil::Time rtLoggingBacklogRetentionTime;
/// @brief The maximum number of entries in the backlog.
@@ -316,7 +342,7 @@ namespace armarx::RobotUnitModule
/// @brief Enable/disable the backlog.
bool rtLoggingBacklogEnabled;
- /// @brief
+ /// @brief
bool logAllEntries = true;
friend void WriteTo(const auto& dentr,
@@ -325,4 +351,4 @@ namespace armarx::RobotUnitModule
std::size_t fidx,
auto& data);
};
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.cpp
index 28a25a4706505ce721037efff9b6d7348531b97c..11a61acc71b3df3ae35b411194741f76cfb38a48 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.cpp
@@ -20,38 +20,42 @@
* GNU General Public License
*/
-#include <ArmarXCore/core/ArmarXManager.h>
-
#include "RobotUnitModuleManagement.h"
+
+#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/time/TimeUtil.h>
namespace armarx::RobotUnitModule
{
- void Management::_preOnInitRobotUnit()
+ void
+ Management::_preOnInitRobotUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- additionalObjectSchedulerCount = getProperty<std::uint64_t>("AdditionalObjectSchedulerCount").getValue();
- heartbeatRequired = getProperty<bool>("HeartbeatRequired").getValue();
- heartbeatMaxCycleMS = getProperty<long>("HeartbeatMaxCycleMS").getValue();
- heartbeatStartupMarginMS = getProperty<long>("HeartbeatStartupMarginMS").getValue();
+ additionalObjectSchedulerCount =
+ getProperty<std::uint64_t>("AdditionalObjectSchedulerCount").getValue();
+ heartbeatRequired = getProperty<bool>("HeartbeatRequired").getValue();
+ heartbeatMaxCycleMS = getProperty<long>("HeartbeatMaxCycleMS").getValue();
+ heartbeatStartupMarginMS = getProperty<long>("HeartbeatStartupMarginMS").getValue();
getArmarXManager()->increaseSchedulers(static_cast<int>(additionalObjectSchedulerCount));
usingTopic(getProperty<std::string>("AggregatedRobotHealthTopicName").getValue());
}
- void Management::_postFinishControlThreadInitialization()
+ void
+ Management::_postFinishControlThreadInitialization()
{
controlLoopStartTime = TimeUtil::GetTime(true).toMilliSeconds();
}
-
- void Management::aggregatedHeartbeat(RobotHealthState overallHealthState, const Ice::Current&)
+ void
+ Management::aggregatedHeartbeat(RobotHealthState overallHealthState, const Ice::Current&)
{
heartbeatRequired = true;
- if (overallHealthState == RobotHealthState::HealthOK || overallHealthState == RobotHealthState::HealthWarning)
+ if (overallHealthState == RobotHealthState::HealthOK ||
+ overallHealthState == RobotHealthState::HealthWarning)
{
lastHeartbeat = TimeUtil::GetTime(true).toMilliSeconds();
}
}
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h
index 71c9f1caad2438d2c0869dc64e70f01440907de6..c92c56d05435c7a9e104837131cc49666a5e6f50 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h
@@ -27,27 +27,22 @@
namespace armarx::RobotUnitModule
{
- class ManagementPropertyDefinitions: public ModuleBasePropertyDefinitions
+ class ManagementPropertyDefinitions : public ModuleBasePropertyDefinitions
{
public:
- ManagementPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
+ ManagementPropertyDefinitions(std::string prefix) : ModuleBasePropertyDefinitions(prefix)
{
defineOptionalProperty<std::uint64_t>(
- "AdditionalObjectSchedulerCount", 10,
- "Number of ObjectSchedulers to be added");
+ "AdditionalObjectSchedulerCount", 10, "Number of ObjectSchedulers to be added");
defineOptionalProperty<bool>(
- "HeartbeatRequired", true,
- "Whether the Heatbeat is required.");
+ "HeartbeatRequired", true, "Whether the Heatbeat is required.");
+ defineOptionalProperty<long>("HeartbeatMaxCycleMS", 100, "The heartbeats cycle time");
defineOptionalProperty<long>(
- "HeartbeatMaxCycleMS", 100,
- "The heartbeats cycle time");
- defineOptionalProperty<long>(
- "HeartbeatStartupMarginMS", 1000,
- "Startup time for heartbeats");
- defineOptionalProperty<std::string>(
- "AggregatedRobotHealthTopicName", "AggregatedRobotHealthTopic",
- "Name of the AggregatedRobotHealthTopic");
+ "HeartbeatStartupMarginMS", 1000, "Startup time for heartbeats");
+ defineOptionalProperty<std::string>("AggregatedRobotHealthTopicName",
+ "AggregatedRobotHealthTopic",
+ "Name of the AggregatedRobotHealthTopic");
}
};
@@ -61,15 +56,18 @@ namespace armarx::RobotUnitModule
class Management : virtual public ModuleBase, virtual public RobotUnitManagementInterface
{
friend class ModuleBase;
+
public:
/**
* @brief Returns the singleton instance of this class
* @return The singleton instance of this class
*/
- static Management& Instance()
+ static Management&
+ Instance()
{
return ModuleBase::Instance<Management>();
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -85,14 +83,17 @@ namespace armarx::RobotUnitModule
* @brief Returns whether the RobotUnit is running.
* @return Whether the RobotUnit is running.
*/
- bool isRunning(const Ice::Current& = Ice::emptyCurrent) const override
+ bool
+ isRunning(const Ice::Current& = Ice::emptyCurrent) const override
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return getRobotUnitState() == RobotUnitState::Running;
}
+
void aggregatedHeartbeat(RobotHealthState overallHealthState, const Ice::Current&) override;
- bool isSimulation(const Ice::Current& = Ice::emptyCurrent) const override
+ bool
+ isSimulation(const Ice::Current& = Ice::emptyCurrent) const override
{
return false;
}
@@ -102,9 +103,9 @@ namespace armarx::RobotUnitModule
// //////////////////////////////////////////////////////////////////////////////////////// //
private:
/// @brief The number of additional object schedulers
- std::int64_t additionalObjectSchedulerCount {0};
- std::int64_t controlLoopStartTime {0};
- std::int64_t heartbeatStartupMarginMS {0};
+ std::int64_t additionalObjectSchedulerCount{0};
+ std::int64_t controlLoopStartTime{0};
+ std::int64_t heartbeatStartupMarginMS{0};
std::atomic_bool heartbeatRequired{false};
std::atomic_long heartbeatMaxCycleMS{100};
std::atomic_long lastHeartbeat{0};
@@ -119,4 +120,4 @@ namespace armarx::RobotUnitModule
*/
friend class ManagementAttorneyForControlThread;
};
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
index 243950a7c5a15f376f1f5c7b09b8b99fac0ea2d2..5d51c4ca2474c8e46bd3be0140ae45ec9e877e55 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
@@ -22,20 +22,20 @@
#include "RobotUnitModulePublisher.h"
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
-#include <RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/util/StringHelpers.h"
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h>
+#include <RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h>
+
namespace armarx::RobotUnitModule
{
/**
@@ -45,37 +45,44 @@ namespace armarx::RobotUnitModule
class NJointControllerAttorneyForPublisher
{
friend class Publisher;
- static bool GetStatusReportedActive(const NJointControllerBasePtr& nJointCtrl)
+
+ static bool
+ GetStatusReportedActive(const NJointControllerBasePtr& nJointCtrl)
{
return nJointCtrl->statusReportedActive;
}
- static bool GetStatusReportedRequested(const NJointControllerBasePtr& nJointCtrl)
+
+ static bool
+ GetStatusReportedRequested(const NJointControllerBasePtr& nJointCtrl)
{
return nJointCtrl->statusReportedRequested;
}
- static void UpdateStatusReported(const NJointControllerBasePtr& nJointCtrl)
+
+ static void
+ UpdateStatusReported(const NJointControllerBasePtr& nJointCtrl)
{
nJointCtrl->statusReportedActive = nJointCtrl->isActive.load();
nJointCtrl->statusReportedRequested = nJointCtrl->isRequested.load();
}
- static void Publish(
- const NJointControllerBasePtr& nJointCtrl,
- const SensorAndControl& sac,
- const DebugDrawerInterfacePrx& draw,
- const DebugObserverInterfacePrx& observer
- )
+
+ static void
+ Publish(const NJointControllerBasePtr& nJointCtrl,
+ const SensorAndControl& sac,
+ const DebugDrawerInterfacePrx& draw,
+ const DebugObserverInterfacePrx& observer)
{
- nJointCtrl ->publish(sac, draw, observer);
+ nJointCtrl->publish(sac, draw, observer);
}
- static void DeactivatePublishing(
- const NJointControllerBasePtr& nJointCtrl,
- const DebugDrawerInterfacePrx& draw,
- const DebugObserverInterfacePrx& observer
- )
+
+ static void
+ DeactivatePublishing(const NJointControllerBasePtr& nJointCtrl,
+ const DebugDrawerInterfacePrx& draw,
+ const DebugObserverInterfacePrx& observer)
{
nJointCtrl->deactivatePublish(draw, observer);
}
};
+
/**
* \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref Publisher.
* \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
@@ -83,11 +90,14 @@ namespace armarx::RobotUnitModule
class DevicesAttorneyForPublisher
{
friend class Publisher;
- static const std::string& GetSensorDeviceName(Publisher* p, std::size_t idx)
+
+ static const std::string&
+ GetSensorDeviceName(Publisher* p, std::size_t idx)
{
return p->_module<Devices>().sensorDevices.at(idx)->getDeviceName();
}
};
+
/**
* \brief This class allows minimal access to private members of \ref Units in a sane fashion for \ref Publisher.
* \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
@@ -95,11 +105,14 @@ namespace armarx::RobotUnitModule
class UnitsAttorneyForPublisher
{
friend class Publisher;
- static const std::vector<RobotUnitSubUnitPtr>& GetSubUnits(Publisher* p)
+
+ static const std::vector<RobotUnitSubUnitPtr>&
+ GetSubUnits(Publisher* p)
{
return p->_module<Units>().subUnits;
}
};
+
/**
* \brief This class allows minimal access to private members of \ref ControllerManagement in a sane fashion for \ref Publisher.
* \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
@@ -107,15 +120,20 @@ namespace armarx::RobotUnitModule
class ControllerManagementAttorneyForPublisher
{
friend class Publisher;
- static const std::map<std::string, NJointControllerBasePtr>& GetNJointControllers(Publisher* p)
+
+ static const std::map<std::string, NJointControllerBasePtr>&
+ GetNJointControllers(Publisher* p)
{
return p->_module<ControllerManagement>().nJointControllers;
}
- static void RemoveNJointControllersToBeDeleted(Publisher* p, bool blocking, RobotUnitListenerPrx l)
+
+ static void
+ RemoveNJointControllersToBeDeleted(Publisher* p, bool blocking, RobotUnitListenerPrx l)
{
p->_module<ControllerManagement>().removeNJointControllersToBeDeleted(blocking, l);
}
};
+
/**
* \brief This class allows minimal access to private members of \ref ControlThread in a sane fashion for \ref Publisher.
* \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
@@ -123,16 +141,19 @@ namespace armarx::RobotUnitModule
class ControlThreadAttorneyForPublisher
{
friend class Publisher;
- static void ProcessEmergencyStopRequest(Publisher* p)
+
+ static void
+ ProcessEmergencyStopRequest(Publisher* p)
{
return p->_module<ControlThread>().processEmergencyStopRequest();
}
};
-}
+} // namespace armarx::RobotUnitModule
namespace armarx::RobotUnitModule
{
- TimedVariantPtr Publisher::publishNJointClassNames()
+ TimedVariantPtr
+ Publisher::publishNJointClassNames()
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -152,13 +173,12 @@ namespace armarx::RobotUnitModule
}
const auto end = TimeUtil::GetTime(true);
- return new TimedVariant {TimestampVariant{end - beg}, lastControlThreadTimestamp};
+ return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
}
- TimedVariantPtr Publisher::publishNJointControllerUpdates(
- StringVariantBaseMap& timingMap,
- const SensorAndControl& controlThreadOutputBuffer
- )
+ TimedVariantPtr
+ Publisher::publishNJointControllerUpdates(StringVariantBaseMap& timingMap,
+ const SensorAndControl& controlThreadOutputBuffer)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -168,24 +188,27 @@ namespace armarx::RobotUnitModule
auto debugDrawerBatchPrx = debugDrawerPrx->ice_batchOneway();
auto debugObserverBatchPrx = debugObserverPrx->ice_batchOneway();
NJointControllerStatusSeq allStatus;
- for (const auto& pair : ControllerManagementAttorneyForPublisher::GetNJointControllers(this))
+ for (const auto& pair :
+ ControllerManagementAttorneyForPublisher::GetNJointControllers(this))
{
const auto begInner = TimeUtil::GetTime(true);
const NJointControllerBasePtr& nJointCtrl = pair.second;
//run some hook for active (used for visu)
- NJointControllerAttorneyForPublisher::Publish(nJointCtrl, controlThreadOutputBuffer, debugDrawerBatchPrx, debugObserverBatchPrx);
- if (
- NJointControllerAttorneyForPublisher::GetStatusReportedActive(nJointCtrl) != nJointCtrl->isControllerActive() ||
- NJointControllerAttorneyForPublisher::GetStatusReportedRequested(nJointCtrl) != nJointCtrl->isControllerRequested()
- )
+ NJointControllerAttorneyForPublisher::Publish(
+ nJointCtrl, controlThreadOutputBuffer, debugDrawerBatchPrx, debugObserverBatchPrx);
+ if (NJointControllerAttorneyForPublisher::GetStatusReportedActive(nJointCtrl) !=
+ nJointCtrl->isControllerActive() ||
+ NJointControllerAttorneyForPublisher::GetStatusReportedRequested(nJointCtrl) !=
+ nJointCtrl->isControllerRequested())
{
NJointControllerAttorneyForPublisher::UpdateStatusReported(nJointCtrl);
allStatus.emplace_back(nJointCtrl->getControllerStatus());
}
const auto endInner = TimeUtil::GetTime(true);
- timingMap["NJointControllerUpdates_" + nJointCtrl->getInstanceName()] = new TimedVariant {TimestampVariant{endInner - begInner}, lastControlThreadTimestamp};
+ timingMap["NJointControllerUpdates_" + nJointCtrl->getInstanceName()] =
+ new TimedVariant{TimestampVariant{endInner - begInner}, lastControlThreadTimestamp};
}
robotUnitListenerBatchPrx->nJointControllerStatusChanged(allStatus);
@@ -193,13 +216,13 @@ namespace armarx::RobotUnitModule
debugDrawerBatchPrx->ice_flushBatchRequests();
const auto end = TimeUtil::GetTime(true);
- return new TimedVariant {TimestampVariant{end - beg}, lastControlThreadTimestamp};
+ return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
}
- TimedVariantPtr Publisher::publishUnitUpdates(
- StringVariantBaseMap& timingMap,
- const SensorAndControl& controlThreadOutputBuffer,
- const JointAndNJointControllers& activatedControllers)
+ TimedVariantPtr
+ Publisher::publishUnitUpdates(StringVariantBaseMap& timingMap,
+ const SensorAndControl& controlThreadOutputBuffer,
+ const JointAndNJointControllers& activatedControllers)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -214,20 +237,21 @@ namespace armarx::RobotUnitModule
const auto begInner = TimeUtil::GetTime(true);
rsu->update(controlThreadOutputBuffer, activatedControllers);
const auto endInner = TimeUtil::GetTime(true);
- timingMap["UnitUpdate_" + rsu->getName()] = new TimedVariant {TimestampVariant{endInner - begInner}, lastControlThreadTimestamp};
+ timingMap["UnitUpdate_" + rsu->getName()] = new TimedVariant{
+ TimestampVariant{endInner - begInner}, lastControlThreadTimestamp};
}
}
const auto end = TimeUtil::GetTime(true);
- return new TimedVariant {TimestampVariant{end - beg}, lastControlThreadTimestamp};
+ return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
}
- TimedVariantPtr Publisher::publishControlUpdates(
- const SensorAndControl& controlThreadOutputBuffer,
- bool haveSensorAndControlValsChanged,
- bool publishToObserver,
- const JointAndNJointControllers& activatedControllers,
- const std::vector<JointController*>& requestedJointControllers)
+ TimedVariantPtr
+ Publisher::publishControlUpdates(const SensorAndControl& controlThreadOutputBuffer,
+ bool haveSensorAndControlValsChanged,
+ bool publishToObserver,
+ const JointAndNJointControllers& activatedControllers,
+ const std::vector<JointController*>& requestedJointControllers)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -236,21 +260,26 @@ namespace armarx::RobotUnitModule
StringVariantBaseMap ctrlDevMap;
ControlDeviceStatusSeq allStatus;
- for (std::size_t ctrlidx = 0 ; ctrlidx < _module<Devices>().getNumberOfControlDevices(); ++ctrlidx)
+ for (std::size_t ctrlidx = 0; ctrlidx < _module<Devices>().getNumberOfControlDevices();
+ ++ctrlidx)
{
const ControlDevice& ctrlDev = *(_module<Devices>().getControlDevices().at(ctrlidx));
StringToStringVariantBaseMapMap variants;
ControlDeviceStatus status;
const auto activeJointCtrl = activatedControllers.jointControllers.at(ctrlidx);
- status.activeControlMode = activeJointCtrl ? activeJointCtrl->getControlMode() : std::string {"!!JointController is nullptr!!"};
+ status.activeControlMode = activeJointCtrl
+ ? activeJointCtrl->getControlMode()
+ : std::string{"!!JointController is nullptr!!"};
status.deviceName = ctrlDev.getDeviceName();
if (activeJointCtrl)
{
- auto additionalDatafields = activeJointCtrl->publish(debugDrawerPrx, debugObserverPrx);
+ auto additionalDatafields =
+ activeJointCtrl->publish(debugDrawerPrx, debugObserverPrx);
for (auto& pair : additionalDatafields)
{
- ctrlDevMap[ctrlDev.getDeviceName() + "_" + activeJointCtrl->getControlMode() + "_" + pair.first] = pair.second;
+ ctrlDevMap[ctrlDev.getDeviceName() + "_" + activeJointCtrl->getControlMode() +
+ "_" + pair.first] = pair.second;
}
}
@@ -258,12 +287,8 @@ namespace armarx::RobotUnitModule
{
const auto& controlMode = ctrlVal->getControlMode();
variants[controlMode] = ctrlVal->toVariants(lastControlThreadTimestamp);
- if (
- haveSensorAndControlValsChanged &&
- !variants[controlMode].empty() &&
- observerPublishControlTargets &&
- publishToObserver
- )
+ if (haveSensorAndControlValsChanged && !variants[controlMode].empty() &&
+ observerPublishControlTargets && publishToObserver)
{
for (const auto& pair : variants[controlMode])
{
@@ -284,17 +309,18 @@ namespace armarx::RobotUnitModule
{
if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
{
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(robotUnitObserver->controlDevicesChannel, std::move(ctrlDevMap));
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(
+ robotUnitObserver->controlDevicesChannel, std::move(ctrlDevMap));
}
}
const auto end = TimeUtil::GetTime(true);
- return new TimedVariant {TimestampVariant{end - beg}, lastControlThreadTimestamp};
+ return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
}
- TimedVariantPtr Publisher::publishSensorUpdates(
- bool publishToObserver,
- const SensorAndControl& controlThreadOutputBuffer)
+ TimedVariantPtr
+ Publisher::publishSensorUpdates(bool publishToObserver,
+ const SensorAndControl& controlThreadOutputBuffer)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -302,9 +328,10 @@ namespace armarx::RobotUnitModule
StringVariantBaseMap sensorDevMap;
const auto numSensorDev = _module<Devices>().getNumberOfSensorDevices();
- ARMARX_CHECK_EQUAL(controlThreadOutputBuffer.sensors.size(), _module<Devices>().getNumberOfSensorDevices());
+ ARMARX_CHECK_EQUAL(controlThreadOutputBuffer.sensors.size(),
+ _module<Devices>().getNumberOfSensorDevices());
SensorDeviceStatusSeq allStatus;
- for (std::size_t sensidx = 0 ; sensidx < numSensorDev; ++sensidx)
+ for (std::size_t sensidx = 0; sensidx < numSensorDev; ++sensidx)
{
ARMARX_TRACE;
const SensorValueBase& sensVal = *(controlThreadOutputBuffer.sensors.at(sensidx));
@@ -340,46 +367,60 @@ namespace armarx::RobotUnitModule
std::stringstream s;
for (auto& pair : sensorDevMap)
{
- s << pair.first << ": \t" << (pair.second ? pair.second->ice_id() + "\t with datatype \t" + Variant::typeToString(pair.second->getType()) : "NULL") << "\n";
+ s << pair.first << ": \t"
+ << (pair.second ? pair.second->ice_id() + "\t with datatype \t" +
+ Variant::typeToString(pair.second->getType())
+ : "NULL")
+ << "\n";
}
- ARMARX_DEBUG << deactivateSpam(spamdelay) << "Sensor Datafieldnames: " << ARMARX_STREAM_PRINTER
+ ARMARX_DEBUG << deactivateSpam(spamdelay)
+ << "Sensor Datafieldnames: " << ARMARX_STREAM_PRINTER
{
for (auto& pair : sensorDevMap)
{
- out << pair.first << ": " << (pair.second ? pair.second->ice_id() + Variant::typeToString(pair.second->getType()) : "NULL") << "\n";
+ out << pair.first << ": "
+ << (pair.second ? pair.second->ice_id() +
+ Variant::typeToString(pair.second->getType())
+ : "NULL")
+ << "\n";
}
};
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(robotUnitObserver->sensorDevicesChannel, std::move(sensorDevMap));
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(
+ robotUnitObserver->sensorDevicesChannel, std::move(sensorDevMap));
}
}
ARMARX_TRACE;
const auto end = TimeUtil::GetTime(true);
- return new TimedVariant {TimestampVariant{end - beg}, lastControlThreadTimestamp};
+ return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
}
- std::string Publisher::getRobotUnitListenerTopicName(const Ice::Current&) const
+ std::string
+ Publisher::getRobotUnitListenerTopicName(const Ice::Current&) const
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return robotUnitListenerTopicName;
}
- std::string Publisher::getDebugDrawerTopicName(const Ice::Current&) const
+ std::string
+ Publisher::getDebugDrawerTopicName(const Ice::Current&) const
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return debugDrawerUpdatesTopicName;
}
- std::string Publisher::getDebugObserverTopicName(const Ice::Current&) const
+ std::string
+ Publisher::getDebugObserverTopicName(const Ice::Current&) const
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return debugObserverTopicName;
}
- RobotUnitListenerPrx Publisher::getRobotUnitListenerProxy(const Ice::Current&) const
+ RobotUnitListenerPrx
+ Publisher::getRobotUnitListenerProxy(const Ice::Current&) const
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -387,7 +428,8 @@ namespace armarx::RobotUnitModule
return robotUnitListenerPrx;
}
- DebugObserverInterfacePrx Publisher::getDebugObserverProxy(const Ice::Current&) const
+ DebugObserverInterfacePrx
+ Publisher::getDebugObserverProxy(const Ice::Current&) const
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -395,7 +437,8 @@ namespace armarx::RobotUnitModule
return debugObserverPrx;
}
- DebugDrawerInterfacePrx Publisher::getDebugDrawerProxy(const Ice::Current&) const
+ DebugDrawerInterfacePrx
+ Publisher::getDebugDrawerProxy(const Ice::Current&) const
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -403,34 +446,45 @@ namespace armarx::RobotUnitModule
return debugDrawerPrx;
}
- void Publisher::_preOnConnectRobotUnit()
+ void
+ Publisher::_preOnConnectRobotUnit()
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_DEBUG << "retrieving RobotUnitListenerPrx for topic " << getRobotUnitListenerTopicName();
+ ARMARX_DEBUG << "retrieving RobotUnitListenerPrx for topic "
+ << getRobotUnitListenerTopicName();
robotUnitListenerPrx = getTopic<RobotUnitListenerPrx>(getRobotUnitListenerTopicName());
robotUnitListenerBatchPrx = robotUnitListenerPrx->ice_batchOneway();
- ARMARX_DEBUG << "retrieving DebugDrawerInterfacePrx for topic " << getDebugDrawerTopicName();
+ ARMARX_DEBUG << "retrieving DebugDrawerInterfacePrx for topic "
+ << getDebugDrawerTopicName();
debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getDebugDrawerTopicName());
- ARMARX_DEBUG << "retrieving DebugObserverInterfacePrx for topic " << getDebugObserverTopicName();
+ ARMARX_DEBUG << "retrieving DebugObserverInterfacePrx for topic "
+ << getDebugObserverTopicName();
debugObserverPrx = getTopic<DebugObserverInterfacePrx>(getDebugObserverTopicName());
}
- void Publisher::_preOnInitRobotUnit()
+ void
+ Publisher::_preOnInitRobotUnit()
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- robotUnitListenerTopicName = getProperty<std::string>("RobotUnitListenerTopicName").getValue();
- debugDrawerUpdatesTopicName = getProperty<std::string>("DebugDrawerUpdatesTopicName").getValue();
+ robotUnitListenerTopicName =
+ getProperty<std::string>("RobotUnitListenerTopicName").getValue();
+ debugDrawerUpdatesTopicName =
+ getProperty<std::string>("DebugDrawerUpdatesTopicName").getValue();
debugObserverTopicName = getProperty<std::string>("DebugObserverTopicName").getValue();
observerPublishSensorValues = getProperty<bool>("ObserverPublishSensorValues").getValue();
- observerPublishControlTargets = getProperty<bool>("ObserverPublishControlTargets").getValue();
- observerPublishTimingInformation = getProperty<bool>("ObserverPublishTimingInformation").getValue();
- observerPublishAdditionalInformation = getProperty<bool>("ObserverPublishAdditionalInformation").getValue();
- debugObserverSkipIterations = std::max(1ul, getProperty<std::uint64_t>("ObserverPrintEveryNthIterations").getValue());
+ observerPublishControlTargets =
+ getProperty<bool>("ObserverPublishControlTargets").getValue();
+ observerPublishTimingInformation =
+ getProperty<bool>("ObserverPublishTimingInformation").getValue();
+ observerPublishAdditionalInformation =
+ getProperty<bool>("ObserverPublishAdditionalInformation").getValue();
+ debugObserverSkipIterations =
+ std::max(1ul, getProperty<std::uint64_t>("ObserverPrintEveryNthIterations").getValue());
publishPeriodMs = std::max(1ul, getProperty<std::size_t>("PublishPeriodMs").getValue());
@@ -440,41 +494,50 @@ namespace armarx::RobotUnitModule
getArmarXManager()->addObject(robotUnitObserver);
}
- void Publisher::_icePropertiesInitialized()
+ void
+ Publisher::_icePropertiesInitialized()
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- robotUnitObserver = Component::create<RobotUnitObserver, RobotUnitObserverPtr>(getIceProperties(), "", getConfigDomain());
+ robotUnitObserver = Component::create<RobotUnitObserver, RobotUnitObserverPtr>(
+ getIceProperties(), "", getConfigDomain());
addPropertyUser(PropertyUserPtr::dynamicCast(robotUnitObserver));
}
- void Publisher::_componentPropertiesUpdated(const std::set<std::string>& changedProperties)
+ void
+ Publisher::_componentPropertiesUpdated(const std::set<std::string>& changedProperties)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (changedProperties.count("ObserverPublishSensorValues"))
{
- observerPublishSensorValues = getProperty<bool>("ObserverPublishSensorValues").getValue();
+ observerPublishSensorValues =
+ getProperty<bool>("ObserverPublishSensorValues").getValue();
}
if (changedProperties.count("ObserverPublishControlTargets"))
{
- observerPublishControlTargets = getProperty<bool>("ObserverPublishControlTargets").getValue();
+ observerPublishControlTargets =
+ getProperty<bool>("ObserverPublishControlTargets").getValue();
}
if (changedProperties.count("ObserverPublishTimingInformation"))
{
- observerPublishTimingInformation = getProperty<bool>("ObserverPublishTimingInformation").getValue();
+ observerPublishTimingInformation =
+ getProperty<bool>("ObserverPublishTimingInformation").getValue();
}
if (changedProperties.count("ObserverPublishAdditionalInformation"))
{
- observerPublishAdditionalInformation = getProperty<bool>("ObserverPublishAdditionalInformation").getValue();
+ observerPublishAdditionalInformation =
+ getProperty<bool>("ObserverPublishAdditionalInformation").getValue();
}
if (changedProperties.count("ObserverPrintEveryNthIterations"))
{
- debugObserverSkipIterations = getProperty<std::uint64_t>("ObserverPrintEveryNthIterations").getValue();
+ debugObserverSkipIterations =
+ getProperty<std::uint64_t>("ObserverPrintEveryNthIterations").getValue();
}
}
- void Publisher::_preFinishRunning()
+ void
+ Publisher::_preFinishRunning()
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -482,35 +545,41 @@ namespace armarx::RobotUnitModule
{
ARMARX_DEBUG << "shutting down publisher task";
publisherTask->stop();
- std::this_thread::sleep_for(std::chrono::milliseconds {10});
+ std::this_thread::sleep_for(std::chrono::milliseconds{10});
while (publisherTask->isFunctionExecuting() || publisherTask->isRunning())
{
- ARMARX_FATAL << deactivateSpam(0.1) << "PUBLISHER TASK IS RUNNING AFTER IT WAS STOPPED!";
+ ARMARX_FATAL << deactivateSpam(0.1)
+ << "PUBLISHER TASK IS RUNNING AFTER IT WAS STOPPED!";
}
publisherTask = nullptr;
//since the drawer queues draw events and we want to clear the layers, we have to sleep here
- std::this_thread::sleep_for(std::chrono::milliseconds {100});
+ std::this_thread::sleep_for(std::chrono::milliseconds{100});
ARMARX_DEBUG << "shutting down publisher task done";
}
- for (const auto& pair : ControllerManagementAttorneyForPublisher::GetNJointControllers(this))
+ for (const auto& pair :
+ ControllerManagementAttorneyForPublisher::GetNJointControllers(this))
{
ARMARX_DEBUG << "forcing deactivation of publishing for " << pair.first;
- NJointControllerAttorneyForPublisher::DeactivatePublishing(pair.second, debugDrawerPrx, debugObserverPrx);
+ NJointControllerAttorneyForPublisher::DeactivatePublishing(
+ pair.second, debugDrawerPrx, debugObserverPrx);
}
}
- void Publisher::_postFinishControlThreadInitialization()
+ void
+ Publisher::_postFinishControlThreadInitialization()
{
ARMARX_TRACE;
//start publisher
publishNewSensorDataTime = TimeUtil::GetTime(true);
- publisherTask = new PublisherTaskT([&] {publish({});}, publishPeriodMs, false, getName() + "_PublisherTask");
+ publisherTask = new PublisherTaskT(
+ [&] { publish({}); }, publishPeriodMs, false, getName() + "_PublisherTask");
ARMARX_INFO << "starting publisher with timestep " << publishPeriodMs;
publisherTask->setDelayWarningTolerance(10 * publishPeriodMs);
publisherTask->start();
}
- void Publisher::publish(StringVariantBaseMap additionalMap, StringVariantBaseMap timingMap)
+ void
+ Publisher::publish(StringVariantBaseMap additionalMap, StringVariantBaseMap timingMap)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -518,7 +587,8 @@ namespace armarx::RobotUnitModule
if (getRobotUnitState() != RobotUnitState::Running)
{
- ARMARX_DEBUG << deactivateSpam(spamdelay) << "skipping publishing in state " << getRobotUnitState();
+ ARMARX_DEBUG << deactivateSpam(spamdelay) << "skipping publishing in state "
+ << getRobotUnitState();
return;
}
if (isShuttingDown())
@@ -542,93 +612,113 @@ namespace armarx::RobotUnitModule
//get batch proxies
//delete NJoint queued for deletion
- ControllerManagementAttorneyForPublisher::RemoveNJointControllersToBeDeleted(this, false, robotUnitListenerBatchPrx);
+ ControllerManagementAttorneyForPublisher::RemoveNJointControllersToBeDeleted(
+ this, false, robotUnitListenerBatchPrx);
//swap buffers in
- const bool haveActivatedControllersChanged = _module<ControlThreadDataBuffer>().activatedControllersChanged();
- const bool haveSensorAndControlValsChanged = _module<ControlThreadDataBuffer>().sensorAndControlBufferChanged();
+ const bool haveActivatedControllersChanged =
+ _module<ControlThreadDataBuffer>().activatedControllersChanged();
+ const bool haveSensorAndControlValsChanged =
+ _module<ControlThreadDataBuffer>().sensorAndControlBufferChanged();
//setup datastructures
- const auto& controlThreadOutputBuffer = _module<ControlThreadDataBuffer>().getSensorAndControlBuffer(); ///TODO copy buffer
+ const auto& controlThreadOutputBuffer =
+ _module<ControlThreadDataBuffer>().getSensorAndControlBuffer(); ///TODO copy buffer
- const auto activatedControllers = _module<ControlThreadDataBuffer>().getActivatedControllers();
- const auto requestedJointControllers = _module<ControlThreadDataBuffer>().copyRequestedJointControllers();
+ const auto activatedControllers =
+ _module<ControlThreadDataBuffer>().getActivatedControllers();
+ const auto requestedJointControllers =
+ _module<ControlThreadDataBuffer>().copyRequestedJointControllers();
lastControlThreadTimestamp = controlThreadOutputBuffer.sensorValuesTimestamp;
const bool publishToObserver = !(publishIterationCount % debugObserverSkipIterations);
//publish publishing meta state
- additionalMap["haveActivatedControllersChanged"] = new TimedVariant {haveActivatedControllersChanged, lastControlThreadTimestamp};
- additionalMap["haveSensorAndControlValsChanged"] = new TimedVariant {haveSensorAndControlValsChanged, lastControlThreadTimestamp};
- additionalMap["publishToObserver" ] = new TimedVariant {publishToObserver, lastControlThreadTimestamp};
- additionalMap["SoftwareEmergencyStopState" ] = new TimedVariant {_module<Units>().getEmergencyStopMaster()->getEmergencyStopState() == EmergencyStopState::eEmergencyStopActive ? "EmergencyStopActive" : "EmergencyStopInactive",
- lastControlThreadTimestamp
- };
+ additionalMap["haveActivatedControllersChanged"] =
+ new TimedVariant{haveActivatedControllersChanged, lastControlThreadTimestamp};
+ additionalMap["haveSensorAndControlValsChanged"] =
+ new TimedVariant{haveSensorAndControlValsChanged, lastControlThreadTimestamp};
+ additionalMap["publishToObserver"] =
+ new TimedVariant{publishToObserver, lastControlThreadTimestamp};
+ additionalMap["SoftwareEmergencyStopState"] =
+ new TimedVariant{_module<Units>().getEmergencyStopMaster()->getEmergencyStopState() ==
+ EmergencyStopState::eEmergencyStopActive
+ ? "EmergencyStopActive"
+ : "EmergencyStopInactive",
+ lastControlThreadTimestamp};
//update
if (haveSensorAndControlValsChanged)
{
- timingMap["UnitUpdate"] = publishUnitUpdates(timingMap, controlThreadOutputBuffer, activatedControllers);
- timingMap["SensorUpdates"] = publishSensorUpdates(publishToObserver, controlThreadOutputBuffer);
+ timingMap["UnitUpdate"] =
+ publishUnitUpdates(timingMap, controlThreadOutputBuffer, activatedControllers);
+ timingMap["SensorUpdates"] =
+ publishSensorUpdates(publishToObserver, controlThreadOutputBuffer);
}
else
{
- const double sensSecondsAgo = (TimeUtil::GetTime() - publishNewSensorDataTime).toSecondsDouble();
+ const double sensSecondsAgo =
+ (TimeUtil::GetTime() - publishNewSensorDataTime).toSecondsDouble();
if (sensSecondsAgo > 1)
{
- ARMARX_WARNING << deactivateSpam(spamdelay) << "armarx::RobotUnit::publish: Last sensor value update is "
+ ARMARX_WARNING << deactivateSpam(spamdelay)
+ << "armarx::RobotUnit::publish: Last sensor value update is "
<< sensSecondsAgo << " seconds ago!";
}
}
if (haveSensorAndControlValsChanged || haveActivatedControllersChanged)
{
- timingMap["ControlUpdates"] = publishControlUpdates(
- controlThreadOutputBuffer,
- haveSensorAndControlValsChanged, publishToObserver,
- activatedControllers, requestedJointControllers);
+ timingMap["ControlUpdates"] = publishControlUpdates(controlThreadOutputBuffer,
+ haveSensorAndControlValsChanged,
+ publishToObserver,
+ activatedControllers,
+ requestedJointControllers);
}
//call publish hook + publish NJointControllerBase changes
- timingMap["NJointControllerUpdates"] = publishNJointControllerUpdates(timingMap, controlThreadOutputBuffer);
+ timingMap["NJointControllerUpdates"] =
+ publishNJointControllerUpdates(timingMap, controlThreadOutputBuffer);
//report new class names
timingMap["ClassNameUpdates"] = publishNJointClassNames();
- timingMap["RobotUnitListenerFlush"] = new TimedVariant
- {
- ARMARX_STOPWATCH(TimestampVariant)
- {
- robotUnitListenerBatchPrx->ice_flushBatchRequests();
- }, lastControlThreadTimestamp
- };
+ timingMap["RobotUnitListenerFlush"] = new TimedVariant{
+ ARMARX_STOPWATCH(TimestampVariant){robotUnitListenerBatchPrx->ice_flushBatchRequests();
+ }
+ , lastControlThreadTimestamp
+}; // namespace armarx::RobotUnitModule
- if (publishToObserver)
+if (publishToObserver)
+{
+ timingMap["LastPublishLoop"] =
+ new TimedVariant{TimestampVariant{lastPublishLoop}, lastControlThreadTimestamp};
+ if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
+ {
+ if (observerPublishTimingInformation)
{
- timingMap["LastPublishLoop"] = new TimedVariant {TimestampVariant{lastPublishLoop}, lastControlThreadTimestamp};
- if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
- {
- if (observerPublishTimingInformation)
- {
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(robotUnitObserver->additionalChannel, std::move(additionalMap));
- }
- if (observerPublishAdditionalInformation)
- {
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(robotUnitObserver->timingChannel, std::move(timingMap));
- }
- }
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(
+ robotUnitObserver->additionalChannel, std::move(additionalMap));
}
-
- //warn if there are unset jointCtrl controllers
+ if (observerPublishAdditionalInformation)
{
- const auto& jointCtrls = _module<ControlThreadDataBuffer>().getActivatedJointControllers();
- if (std::any_of(jointCtrls.begin(), jointCtrls.end(), [](const JointController * jointCtrl)
- {
- return !jointCtrl;
- }))
- {
- ARMARX_WARNING << deactivateSpam(5) << "armarx::RobotUnit::publish: Some activated JointControllers are reported to be nullptr! "
- << "(did you forget to call rtSwitchControllerSetup()?)";
- }
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(
+ robotUnitObserver->timingChannel, std::move(timingMap));
}
- ++publishIterationCount;
- lastPublishLoop = TimeUtil::GetTime(true) - begPublish;
}
}
+
+//warn if there are unset jointCtrl controllers
+{
+ const auto& jointCtrls = _module<ControlThreadDataBuffer>().getActivatedJointControllers();
+ if (std::any_of(jointCtrls.begin(),
+ jointCtrls.end(),
+ [](const JointController* jointCtrl) { return !jointCtrl; }))
+ {
+ ARMARX_WARNING << deactivateSpam(5)
+ << "armarx::RobotUnit::publish: Some activated JointControllers are "
+ "reported to be nullptr! "
+ << "(did you forget to call rtSwitchControllerSetup()?)";
+ }
+}
+++publishIterationCount;
+lastPublishLoop = TimeUtil::GetTime(true) - begPublish;
+}
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
index 9b68b4c7aeffc6ef77b3aa5e0c2d9ca20c1c8677..9f061fd02cad06539fb919e32087912f72713f6e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
@@ -28,8 +28,8 @@
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
-#include "RobotUnitModuleBase.h"
#include "../RobotUnitObserver.h"
+#include "RobotUnitModuleBase.h"
namespace armarx::detail
{
@@ -40,42 +40,47 @@ namespace armarx
{
TYPEDEF_PTRS_HANDLE(RobotUnitObserver);
using SensorAndControl = detail::ControlThreadOutputBufferEntry;
-}
+} // namespace armarx
namespace armarx::RobotUnitModule
{
- class PublisherPropertyDefinitions: public ModuleBasePropertyDefinitions
+ class PublisherPropertyDefinitions : public ModuleBasePropertyDefinitions
{
public:
- PublisherPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
+ PublisherPropertyDefinitions(std::string prefix) : ModuleBasePropertyDefinitions(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::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");
+ "PublishPeriodMs", 10, "Milliseconds between each publish");
- defineOptionalProperty<bool>(
- "ObserverPublishSensorValues", true,
- "Whether sensor values are send to the observer", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<bool>(
- "ObserverPublishControlTargets", true,
- "Whether control targets are send to the observer", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<bool>(
- "ObserverPublishTimingInformation", true,
- "Whether timing information are send to the observer", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<bool>(
- "ObserverPublishAdditionalInformation", true,
- "Whether additional information are send to the observer", PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>("ObserverPublishSensorValues",
+ true,
+ "Whether sensor values are send to the observer",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>("ObserverPublishControlTargets",
+ true,
+ "Whether control targets are send to the observer",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>("ObserverPublishTimingInformation",
+ true,
+ "Whether timing information are send to the observer",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>("ObserverPublishAdditionalInformation",
+ true,
+ "Whether additional information are send to the observer",
+ PropertyDefinitionBase::eModifiable);
defineOptionalProperty<std::string>(
"DebugObserverTopicName", "DebugObserver", "The topic where updates are send to");
defineOptionalProperty<std::uint64_t>(
- "ObserverPrintEveryNthIterations", 1,
- "Only every nth iteration data is send to the debug observer", PropertyDefinitionBase::eModifiable);
+ "ObserverPrintEveryNthIterations",
+ 1,
+ "Only every nth iteration data is send to the debug observer",
+ PropertyDefinitionBase::eModifiable);
}
};
@@ -89,15 +94,18 @@ namespace armarx::RobotUnitModule
class Publisher : virtual public ModuleBase, virtual public RobotUnitPublishingInterface
{
friend class ModuleBase;
+
public:
/**
* @brief Returns the singleton instance of this class
* @return The singleton instance of this class
*/
- static Publisher& Instance()
+ static Publisher&
+ Instance()
{
return ModuleBase::Instance<Publisher>();
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -128,28 +136,33 @@ namespace armarx::RobotUnitModule
* @brief Returns the name of the used DebugObserverTopic
* @return The name of the used DebugObserverTopic
*/
- std::string getDebugObserverTopicName(const Ice::Current& = Ice::emptyCurrent) const override;
+ std::string
+ getDebugObserverTopicName(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the name of the used RobotUnitListenerTopic
* @return The name of the used RobotUnitListenerTopic
*/
- std::string getRobotUnitListenerTopicName(const Ice::Current& = Ice::emptyCurrent) const override;
+ std::string
+ getRobotUnitListenerTopicName(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the used DebugDrawerProxy
* @return The used DebugDrawerProxy
*/
- DebugDrawerInterfacePrx getDebugDrawerProxy(const Ice::Current& = Ice::emptyCurrent) const override;
+ DebugDrawerInterfacePrx
+ getDebugDrawerProxy(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the used RobotUnitListenerProxy
* @return The used RobotUnitListenerProxy
*/
- RobotUnitListenerPrx getRobotUnitListenerProxy(const Ice::Current& = Ice::emptyCurrent) const override;
+ RobotUnitListenerPrx
+ getRobotUnitListenerProxy(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the used DebugObserverProxy
* @return The used DebugObserverProxy
*/
- DebugObserverInterfacePrx getDebugObserverProxy(const Ice::Current& = Ice::emptyCurrent) const override;
+ DebugObserverInterfacePrx
+ getDebugObserverProxy(const Ice::Current& = Ice::emptyCurrent) const override;
// //////////////////////////////////////////////////////////////////////////////////////// //
// /////////////////////////////////// Module interface /////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -159,7 +172,8 @@ namespace armarx::RobotUnitModule
* @param additionalMap Data published to the RobotUnitObserver's additional channel (This field is used by deriving classes)
* @param timingMap Data published to the RobotUnitObserver's timing channel (This field is used by deriving classes)
*/
- virtual void publish(StringVariantBaseMap additionalMap = {}, StringVariantBaseMap timingMap = {});
+ virtual void publish(StringVariantBaseMap additionalMap = {},
+ StringVariantBaseMap timingMap = {});
// //////////////////////////////////////////////////////////////////////////////////////// //
// //////////////////////////////////// implementation //////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -168,11 +182,13 @@ namespace armarx::RobotUnitModule
* @brief Returns the used RobotUnitObserver
* @return The used RobotUnitObserver
*/
- const RobotUnitObserverPtr& getRobotUnitObserver() const
+ const RobotUnitObserverPtr&
+ getRobotUnitObserver() const
{
ARMARX_CHECK_EXPRESSION(robotUnitObserver);
return robotUnitObserver;
}
+
/**
* @brief Publishes updtes about new classes od \ref NJointControllerBase "NJointControllers"
* @return The time required by this function as a \ref Variant
@@ -185,9 +201,9 @@ namespace armarx::RobotUnitModule
* @return The time required by this function as a \ref Variant
* @see NJointControllerBase::onPublish
*/
- TimedVariantPtr publishNJointControllerUpdates(
- StringVariantBaseMap& timingMap,
- const SensorAndControl& controlThreadOutputBuffer);
+ TimedVariantPtr
+ publishNJointControllerUpdates(StringVariantBaseMap& timingMap,
+ const SensorAndControl& controlThreadOutputBuffer);
/**
* @brief Updates all sub units and publishes the timings of these updates
* @param timingMap Timings of this publish iteration (out param)
@@ -196,10 +212,9 @@ namespace armarx::RobotUnitModule
* active in the published \ref ControlThread iteration
* @return The time required by this function as a \ref Variant
*/
- TimedVariantPtr publishUnitUpdates(
- StringVariantBaseMap& timingMap,
- const SensorAndControl& controlThreadOutputBuffer,
- const JointAndNJointControllers& activatedControllers);
+ TimedVariantPtr publishUnitUpdates(StringVariantBaseMap& timingMap,
+ const SensorAndControl& controlThreadOutputBuffer,
+ const JointAndNJointControllers& activatedControllers);
/**
* @brief Publishes data about updates of \ref JointController "JointControllers" and their \ref ControlTargetBase "ControlTargets"
* @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
@@ -210,21 +225,20 @@ namespace armarx::RobotUnitModule
* @param requestedJointControllers
* @return The time required by this function as a \ref Variant
*/
- TimedVariantPtr publishControlUpdates(
- const SensorAndControl& controlThreadOutputBuffer,
- bool haveSensorAndControlValsChanged,
- bool publishToObserver,
- const JointAndNJointControllers& activatedControllers,
- const std::vector<JointController*>& requestedJointControllers);
+ TimedVariantPtr
+ publishControlUpdates(const SensorAndControl& controlThreadOutputBuffer,
+ bool haveSensorAndControlValsChanged,
+ bool publishToObserver,
+ const JointAndNJointControllers& activatedControllers,
+ const std::vector<JointController*>& requestedJointControllers);
/**
* @brief Publishes Updates about \ref SensorValueBase "SensorValues" (To \ref RobotUnitListener and \ref RobotUnitObserver)
* @param publishToObserver Whether data should be published to observers
* @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
* @return The time required by this function as a \ref Variant
*/
- TimedVariantPtr publishSensorUpdates(
- bool publishToObserver,
- const SensorAndControl& controlThreadOutputBuffer);
+ TimedVariantPtr publishSensorUpdates(bool publishToObserver,
+ const SensorAndControl& controlThreadOutputBuffer);
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////////////// Data ///////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -246,7 +260,7 @@ namespace armarx::RobotUnitModule
DebugObserverInterfacePrx debugObserverPrx;
/// @brief the number of calles to \ref publish
- std::uint64_t publishIterationCount {0};
+ std::uint64_t publishIterationCount{0};
/// @brief The time required by the last iteration of \ref publish to publish sensor data
IceUtil::Time publishNewSensorDataTime;
@@ -275,7 +289,7 @@ namespace armarx::RobotUnitModule
RobotUnitObserverPtr robotUnitObserver;
/// @brief The period of the publisher loop
- std::size_t publishPeriodMs {1};
+ std::size_t publishPeriodMs{1};
/// @brief A proxy to the used RobotUnitListener topic
RobotUnitListenerPrx robotUnitListenerPrx;
@@ -286,4 +300,4 @@ namespace armarx::RobotUnitModule
/// \warning May only be accessed by the publish thread.
IceUtil::Time lastControlThreadTimestamp;
};
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
index b60019683741f988d4eefcf131cd44df44b1be9c..a7083b662e54b0a0c02ad847c3e0f197bce74d58 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
@@ -20,66 +20,78 @@
* GNU General Public License
*/
-#include <VirtualRobot/XML/RobotIO.h>
+#include "RobotUnitModuleRobotData.h"
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/XML/RobotIO.h>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <RobotAPI/libraries/core/RobotPool.h>
-#include "RobotUnitModuleRobotData.h"
-
-#include <SimoxUtility/algorithm/string/string_tools.h>
-
namespace armarx::RobotUnitModule
{
- const std::string& RobotData::getRobotPlatformName() const
+ const std::string&
+ RobotData::getRobotPlatformName() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_CHECK_EXPRESSION(robot) << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
+ ARMARX_CHECK_EXPRESSION(robot)
+ << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
return robotPlatformName;
}
- std::string RobotData::getRobotPlatformInstanceName() const
+ std::string
+ RobotData::getRobotPlatformInstanceName() const
{
return robotPlatformInstanceName;
}
- const std::string& RobotData::getRobotNodetSeName() const
+ const std::string&
+ RobotData::getRobotNodetSeName() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_CHECK_EXPRESSION(robot) << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
+ ARMARX_CHECK_EXPRESSION(robot)
+ << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
return robotNodeSetName;
}
- const std::string& RobotData::getRobotProjectName() const
+ const std::string&
+ RobotData::getRobotProjectName() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_CHECK_EXPRESSION(robot) << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
+ ARMARX_CHECK_EXPRESSION(robot)
+ << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
return robotProjectName;
}
- const std::string& RobotData::getRobotFileName() const
+ const std::string&
+ RobotData::getRobotFileName() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_CHECK_EXPRESSION(robot) << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
+ ARMARX_CHECK_EXPRESSION(robot)
+ << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
return robotFileName;
}
- std::string RobotData::getRobotName() const
+ std::string
+ RobotData::getRobotName() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {robotMutex};
- ARMARX_CHECK_EXPRESSION(robot) << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
+ std::lock_guard<std::mutex> guard{robotMutex};
+ ARMARX_CHECK_EXPRESSION(robot)
+ << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
return robot->getName();
}
- VirtualRobot::RobotPtr RobotData::cloneRobot(bool updateCollisionModel) const
+ VirtualRobot::RobotPtr
+ RobotData::cloneRobot(bool updateCollisionModel) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {robotMutex};
- ARMARX_CHECK_EXPRESSION(robot) << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
+ std::lock_guard<std::mutex> guard{robotMutex};
+ ARMARX_CHECK_EXPRESSION(robot)
+ << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
ARMARX_CHECK_EXPRESSION(robotPool);
const VirtualRobot::RobotPtr clone = robotPool->getRobot();
clone->setUpdateVisualization(false);
@@ -87,21 +99,20 @@ namespace armarx::RobotUnitModule
return clone;
}
-
-
- void RobotData::_initVirtualRobot()
+ void
+ RobotData::_initVirtualRobot()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {robotMutex};
+ std::lock_guard<std::mutex> guard{robotMutex};
ARMARX_CHECK_IS_NULL(robot);
- std::string robotName = getProperty<std::string>("RobotName").getValue();
+ std::string robotName = getProperty<std::string>("RobotName").getValue();
- robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
- robotProjectName = getProperty<std::string>("RobotFileNameProject").getValue();
- robotFileName = getProperty<std::string>("RobotFileName").getValue();
- robotPlatformName = getProperty<std::string>("PlatformName").getValue();
- robotPlatformInstanceName = getProperty<std::string>("PlatformInstanceName").getValue();
+ robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
+ robotProjectName = getProperty<std::string>("RobotFileNameProject").getValue();
+ robotFileName = getProperty<std::string>("RobotFileName").getValue();
+ robotPlatformName = getProperty<std::string>("PlatformName").getValue();
+ robotPlatformInstanceName = getProperty<std::string>("PlatformInstanceName").getValue();
//load robot
{
@@ -117,13 +128,15 @@ namespace armarx::RobotUnitModule
{
std::stringstream str;
str << "Could not find robot file " + robotFileName
- << "\nCollected paths from RobotFileNameProject '" << robotProjectName << "':" << includePaths;
+ << "\nCollected paths from RobotFileNameProject '" << robotProjectName
+ << "':" << includePaths;
throw UserException(str.str());
}
// read the robot
try
{
- robot = VirtualRobot::RobotIO::loadRobot(robotFileName, VirtualRobot::RobotIO::eFull);
+ robot =
+ VirtualRobot::RobotIO::loadRobot(robotFileName, VirtualRobot::RobotIO::eFull);
if (robotName.empty())
{
robotName = robot->getName();
@@ -141,10 +154,10 @@ namespace armarx::RobotUnitModule
<< "\n\tProject : " << robotProjectName
<< "\n\tName : " << robotName
<< "\n\tRobot file : " << robotFileName
- << "\n\tRobotNodeSet : " << robotNodeSetName
- << "\n\tNodeNames : " << robot->getRobotNodeSet(robotNodeSetName)->getNodeNames();
+ << "\n\tRobotNodeSet : " << robotNodeSetName << "\n\tNodeNames : "
+ << robot->getRobotNodeSet(robotNodeSetName)->getNodeNames();
ARMARX_CHECK_NOT_NULL(robot);
robotPool.reset(new RobotPool(robot, 10));
}
}
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
index 126aa742114c889b2cef6eeabfdf70302b91bd07..7b5defcbfbec169a83e7185f688a5016851c24ab 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
@@ -24,8 +24,8 @@
#include <VirtualRobot/Robot.h>
-#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/core/ArmarXManager.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/observers/DebugObserver.h>
#include "RobotUnitModuleBase.h"
@@ -37,30 +37,34 @@ namespace armarx
namespace armarx::RobotUnitModule
{
- class RobotDataPropertyDefinitions: public ModuleBasePropertyDefinitions
+ class RobotDataPropertyDefinitions : public ModuleBasePropertyDefinitions
{
public:
- RobotDataPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
+ RobotDataPropertyDefinitions(std::string prefix) : ModuleBasePropertyDefinitions(prefix)
{
- 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)");
+ 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>(
- "RobotName", "",
+ "RobotName",
+ "",
"Override robot name if you want to load multiple robots of the same type");
defineOptionalProperty<std::string>(
- "RobotNodeSetName", "Robot",
+ "RobotNodeSetName",
+ "Robot",
"Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
defineOptionalProperty<std::string>(
- "PlatformName", "Platform",
+ "PlatformName",
+ "Platform",
"Name of the platform needs to correspond to a node in the virtual robot.");
- defineOptionalProperty<std::string>(
- "PlatformInstanceName", "Platform",
- "Name of the platform instance (will publish values on PlatformInstanceName + 'State')");
-
+ defineOptionalProperty<std::string>("PlatformInstanceName",
+ "Platform",
+ "Name of the platform instance (will publish "
+ "values on PlatformInstanceName + 'State')");
}
};
@@ -73,15 +77,18 @@ namespace armarx::RobotUnitModule
class RobotData : virtual public ModuleBase
{
friend class ModuleBase;
+
public:
/**
* @brief Returns the singleton instance of this class
* @return The singleton instance of this class
*/
- static RobotData& Instance()
+ static RobotData&
+ Instance()
{
return ModuleBase::Instance<RobotData>();
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -152,5 +159,4 @@ namespace armarx::RobotUnitModule
/// @brief A mutex guarding \ref robot
mutable std::mutex robotMutex;
};
-}
-
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
index 09b19d38a9ede50f8e7f167aad5aee2681bf30e3..a4d9867488a2a2f8e9cb176158ba18fc01a8c538 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
@@ -20,33 +20,36 @@
* GNU General Public License
*/
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/Obstacle.h>
+#include "RobotUnitModuleSelfCollisionChecker.h"
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
+#include <VirtualRobot/Obstacle.h>
+#include <VirtualRobot/RobotNodeSet.h>
-#include "RobotUnitModuleSelfCollisionChecker.h"
-#include "RobotUnitModuleRobotData.h"
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h>
+#include <ArmarXCore/core/util/OnScopeExit.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
-#include <ArmarXCore/core/util/OnScopeExit.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h>
-#include <SimoxUtility/algorithm/string/string_tools.h>
+#include "RobotUnitModuleRobotData.h"
#define FLOOR_OBJ_STR "FLOOR"
namespace armarx::RobotUnitModule
{
- void SelfCollisionChecker::_preOnInitRobotUnit()
+ void
+ SelfCollisionChecker::_preOnInitRobotUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
//get the robot
selfCollisionAvoidanceRobot = _module<RobotData>().cloneRobot(true);
//get pairs to check
{
- const std::string colModelsString = getProperty<std::string>("SelfCollisionCheck_ModelGroupsToCheck").getValue();
+ const std::string colModelsString =
+ getProperty<std::string>("SelfCollisionCheck_ModelGroupsToCheck").getValue();
std::vector<std::string> groups;
if (!colModelsString.empty())
{
@@ -72,50 +75,60 @@ namespace armarx::RobotUnitModule
{
std::set<std::string> nodes;
//the entry is a set
- for (const auto& node : selfCollisionAvoidanceRobot->getRobotNodeSet(subentry)->getAllRobotNodes())
+ for (const auto& node :
+ selfCollisionAvoidanceRobot->getRobotNodeSet(subentry)
+ ->getAllRobotNodes())
{
if (!node->getCollisionModel())
{
- ARMARX_WARNING << "Self Collision Avoidance: No collision model found for '"
+ ARMARX_WARNING << "Self Collision Avoidance: No collision "
+ "model found for '"
<< node->getName() << "'";
continue;
}
nodes.emplace(node->getName());
- ARMARX_DEBUG << "-------- from set: " << subentry << ", node: " << node->getName();
+ ARMARX_DEBUG << "-------- from set: " << subentry
+ << ", node: " << node->getName();
}
setsOfNode.emplace(std::move(nodes));
}
else if (selfCollisionAvoidanceRobot->hasRobotNode(subentry))
{
//the entry is a node
- if (!selfCollisionAvoidanceRobot->getRobotNode(subentry)->getCollisionModel())
+ if (!selfCollisionAvoidanceRobot->getRobotNode(subentry)
+ ->getCollisionModel())
{
- ARMARX_WARNING << "Self Collision Avoidance: No collision model found for '"
- << selfCollisionAvoidanceRobot->getRobotNode(subentry)->getName() << "'";
+ ARMARX_WARNING
+ << "Self Collision Avoidance: No collision model found for '"
+ << selfCollisionAvoidanceRobot->getRobotNode(subentry)
+ ->getName()
+ << "'";
continue;
}
- setsOfNode.emplace(std::set<std::string> {subentry});
+ setsOfNode.emplace(std::set<std::string>{subentry});
ARMARX_DEBUG << "-------- node: " << subentry;
}
else if (subentry == FLOOR_OBJ_STR)
{
//the entry is the floor
- setsOfNode.emplace(std::set<std::string> {subentry});
+ setsOfNode.emplace(std::set<std::string>{subentry});
ARMARX_DEBUG << "-------- floor: " << subentry;
}
else
{
- ARMARX_ERROR << "No RobotNodeSet or RobotNode with name '"
- << subentry
- << "' defined in " << _module<RobotData>().getRobotFileName() << ". Skipping.";
+ ARMARX_ERROR << "No RobotNodeSet or RobotNode with name '" << subentry
+ << "' defined in "
+ << _module<RobotData>().getRobotFileName()
+ << ". Skipping.";
continue;
}
}
}
- auto addCombinationOfSetsToCollisionCheck = [this](const std::set<std::string>& a, const std::set<std::string>& b)
+ auto addCombinationOfSetsToCollisionCheck =
+ [this](const std::set<std::string>& a, const std::set<std::string>& b)
{
for (const auto& nodeA : a)
{
@@ -155,20 +168,22 @@ namespace armarx::RobotUnitModule
}
ARMARX_DEBUG << "Processing groups for self collision checking...DONE!";
}
- setSelfCollisionAvoidanceFrequency(getProperty<float>("SelfCollisionCheck_Frequency").getValue());
- setSelfCollisionAvoidanceDistance(getProperty<float>("SelfCollisionCheck_MinSelfDistance").getValue());
+ setSelfCollisionAvoidanceFrequency(
+ getProperty<float>("SelfCollisionCheck_Frequency").getValue());
+ setSelfCollisionAvoidanceDistance(
+ getProperty<float>("SelfCollisionCheck_MinSelfDistance").getValue());
}
- void SelfCollisionChecker::setSelfCollisionAvoidanceDistance(Ice::Float distance, const Ice::Current&)
+ void
+ SelfCollisionChecker::setSelfCollisionAvoidanceDistance(Ice::Float distance,
+ const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {selfCollisionDataMutex};
+ std::lock_guard<std::mutex> guard{selfCollisionDataMutex};
if (distance < 0)
{
- throw InvalidArgumentException
- {
- std::string{__FILE__} + ": " + BOOST_CURRENT_FUNCTION + ": illegal distance:" + std::to_string(distance)
- };
+ throw InvalidArgumentException{std::string{__FILE__} + ": " + BOOST_CURRENT_FUNCTION +
+ ": illegal distance:" + std::to_string(distance)};
}
if (distance == minSelfDistance && !nodePairsToCheck.empty())
{
@@ -182,11 +197,16 @@ namespace armarx::RobotUnitModule
nodePairsToCheck.clear();
//set floor
{
- floor.reset(new VirtualRobot::SceneObjectSet("FLOORSET", selfCollisionAvoidanceRobot->getCollisionChecker()));
+ floor.reset(new VirtualRobot::SceneObjectSet(
+ "FLOORSET", selfCollisionAvoidanceRobot->getCollisionChecker()));
static constexpr float floorSize = 1e16f;
- VirtualRobot::ObstaclePtr boxOb = VirtualRobot::Obstacle::createBox(floorSize, floorSize, std::min(0.001f, minSelfDistance / 2),
- VirtualRobot::VisualizationFactory::Color::Red(), "",
- selfCollisionAvoidanceRobot->getCollisionChecker());
+ VirtualRobot::ObstaclePtr boxOb = VirtualRobot::Obstacle::createBox(
+ floorSize,
+ floorSize,
+ std::min(0.001f, minSelfDistance / 2),
+ VirtualRobot::VisualizationFactory::Color::Red(),
+ "",
+ selfCollisionAvoidanceRobot->getCollisionChecker());
boxOb->setGlobalPose(Eigen::Matrix4f::Identity());
boxOb->setName(FLOOR_OBJ_STR);
floor->addSceneObject(boxOb);
@@ -202,75 +222,85 @@ namespace armarx::RobotUnitModule
//collect pairs
for (const auto& pair : namePairsToCheck)
{
- VirtualRobot::SceneObjectPtr first =
- (pair.first == FLOOR_OBJ_STR) ?
- floor->getSceneObject(0) :
- selfCollisionAvoidanceRobot->getRobotNode(pair.first);
+ VirtualRobot::SceneObjectPtr first =
+ (pair.first == FLOOR_OBJ_STR)
+ ? floor->getSceneObject(0)
+ : selfCollisionAvoidanceRobot->getRobotNode(pair.first);
VirtualRobot::SceneObjectPtr second =
- (pair.second == FLOOR_OBJ_STR) ?
- floor->getSceneObject(0) :
- selfCollisionAvoidanceRobot->getRobotNode(pair.second);
+ (pair.second == FLOOR_OBJ_STR)
+ ? floor->getSceneObject(0)
+ : selfCollisionAvoidanceRobot->getRobotNode(pair.second);
nodePairsToCheck.emplace_back(first, second);
}
ARMARX_CHECK_EQUAL(nodePairsToCheck.size(), nodePairsToCheck.size());
}
- void SelfCollisionChecker::setSelfCollisionAvoidanceFrequency(Ice::Float freq, const Ice::Current&)
+ void
+ SelfCollisionChecker::setSelfCollisionAvoidanceFrequency(Ice::Float freq, const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (freq < 0)
{
- throw InvalidArgumentException
- {
- std::string{__FILE__} + ": " + BOOST_CURRENT_FUNCTION + ": illegal frequency:" + std::to_string(freq)
- };
+ throw InvalidArgumentException{std::string{__FILE__} + ": " + BOOST_CURRENT_FUNCTION +
+ ": illegal frequency:" + std::to_string(freq)};
}
checkFrequency = freq;
}
- bool SelfCollisionChecker::isSelfCollisionCheckEnabled(const Ice::Current&) const
+ bool
+ SelfCollisionChecker::isSelfCollisionCheckEnabled(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return checkFrequency != 0;
}
- float SelfCollisionChecker::getSelfCollisionAvoidanceFrequency(const Ice::Current&) const
+ float
+ SelfCollisionChecker::getSelfCollisionAvoidanceFrequency(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return checkFrequency;
}
- float SelfCollisionChecker::getSelfCollisionAvoidanceDistance(const Ice::Current&) const
+ float
+ SelfCollisionChecker::getSelfCollisionAvoidanceDistance(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return minSelfDistance;
}
- void SelfCollisionChecker::_componentPropertiesUpdated(const std::set<std::string>& changed)
+ void
+ SelfCollisionChecker::_componentPropertiesUpdated(const std::set<std::string>& changed)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (changed.count("SelfCollisionCheck_Frequency"))
{
- setSelfCollisionAvoidanceFrequency(getProperty<float>("SelfCollisionCheck_Frequency").getValue());
+ setSelfCollisionAvoidanceFrequency(
+ getProperty<float>("SelfCollisionCheck_Frequency").getValue());
}
if (changed.count("SelfCollisionCheck_MinSelfDistance"))
{
- setSelfCollisionAvoidanceDistance(getProperty<float>("SelfCollisionCheck_MinSelfDistance").getValue());
+ setSelfCollisionAvoidanceDistance(
+ getProperty<float>("SelfCollisionCheck_MinSelfDistance").getValue());
}
}
- void SelfCollisionChecker::_postFinishControlThreadInitialization()
+ void
+ SelfCollisionChecker::_postFinishControlThreadInitialization()
{
- selfCollisionAvoidanceThread = std::thread {[&]{selfCollisionAvoidanceTask();}};
+ selfCollisionAvoidanceThread = std::thread{[&] { selfCollisionAvoidanceTask(); }};
}
- void SelfCollisionChecker::selfCollisionAvoidanceTask()
+ void
+ SelfCollisionChecker::selfCollisionAvoidanceTask()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_INFO << "Self collision checker: entered selfCollisionAvoidanceTask";
- ARMARX_ON_SCOPE_EXIT { ARMARX_INFO << "Self collision checker: leaving selfCollisionAvoidanceTask"; };
+ ARMARX_ON_SCOPE_EXIT
+ {
+ ARMARX_INFO << "Self collision checker: leaving selfCollisionAvoidanceTask";
+ };
while (true)
{
const auto startT = std::chrono::high_resolution_clock::now();
@@ -280,19 +310,22 @@ namespace armarx::RobotUnitModule
return;
}
const auto freq = checkFrequency.load();
- const bool inEmergencyStop = _module<ControlThread>().getEmergencyStopState() == eEmergencyStopActive;
+ const bool inEmergencyStop =
+ _module<ControlThread>().getEmergencyStopState() == eEmergencyStopActive;
if (inEmergencyStop || freq == 0)
{
- ARMARX_INFO << deactivateSpam() << "Self collision checker: skipping check " << VAROUT(freq) << " " << VAROUT(inEmergencyStop);
+ ARMARX_INFO << deactivateSpam() << "Self collision checker: skipping check "
+ << VAROUT(freq) << " " << VAROUT(inEmergencyStop);
//currently wait
- std::this_thread::sleep_for(std::chrono::microseconds {1000});
+ std::this_thread::sleep_for(std::chrono::microseconds{1000});
continue;
}
//update robot + check
{
- std::lock_guard<std::mutex> guard {selfCollisionDataMutex};
+ std::lock_guard<std::mutex> guard{selfCollisionDataMutex};
//update robot
- _module<ControlThreadDataBuffer>().updateVirtualRobot(selfCollisionAvoidanceRobot, selfCollisionAvoidanceRobotNodes);
+ _module<ControlThreadDataBuffer>().updateVirtualRobot(
+ selfCollisionAvoidanceRobot, selfCollisionAvoidanceRobotNodes);
//check for all nodes 0
{
@@ -315,12 +348,16 @@ namespace armarx::RobotUnitModule
for (std::size_t idx = 0; idx < nodePairsToCheck.size(); ++idx)
{
const auto& pair = nodePairsToCheck.at(idx);
- if (selfCollisionAvoidanceRobot->getCollisionChecker()->checkCollision(pair.first, pair.second))
+ if (selfCollisionAvoidanceRobot->getCollisionChecker()->checkCollision(
+ pair.first, pair.second))
{
collision = true;
lastCollisionPairIndex = idx;
- ARMARX_WARNING << "Self collision checker: COLLISION: '" << pair.first->getName() << "' and '" << pair.second->getName() << "'";
- _module<Units>().getEmergencyStopMaster()->setEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
+ ARMARX_WARNING << "Self collision checker: COLLISION: '"
+ << pair.first->getName() << "' and '"
+ << pair.second->getName() << "'";
+ _module<Units>().getEmergencyStopMaster()->setEmergencyStopState(
+ EmergencyStopState::eEmergencyStopActive);
// since at least one of the NJoint-Controllers is going to cause a collision, we just kick them all.
_module<ControlThreadDataBuffer>().setActivateControllersRequest({});
break;
@@ -328,15 +365,19 @@ namespace armarx::RobotUnitModule
}
if (!collision)
{
- ARMARX_VERBOSE << deactivateSpam() << "Self collision checker: no collision found between the " << nodePairsToCheck.size() << " pairs";
+ ARMARX_VERBOSE << deactivateSpam()
+ << "Self collision checker: no collision found between the "
+ << nodePairsToCheck.size() << " pairs";
}
}
//sleep remaining
- std::this_thread::sleep_until(startT + std::chrono::microseconds {static_cast<int64_t>(1000000 / freq)});
+ std::this_thread::sleep_until(
+ startT + std::chrono::microseconds{static_cast<int64_t>(1000000 / freq)});
}
}
- void SelfCollisionChecker::_preFinishRunning()
+ void
+ SelfCollisionChecker::_preFinishRunning()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_INFO << "Stopping Self Collision Avoidance.";
@@ -345,5 +386,4 @@ namespace armarx::RobotUnitModule
selfCollisionAvoidanceThread.join();
}
}
-}
-
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h
index d1d059ea78d90f41b47f3d2aa3f77e9303e73974..fa7e690cd602045f3ed0883e81fb0e3956822b69 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h
@@ -29,30 +29,38 @@
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
#include "RobotUnitModuleBase.h"
namespace armarx::RobotUnitModule
{
- class SelfCollisionCheckerPropertyDefinitions: public ModuleBasePropertyDefinitions
+ class SelfCollisionCheckerPropertyDefinitions : public ModuleBasePropertyDefinitions
{
public:
- SelfCollisionCheckerPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
+ SelfCollisionCheckerPropertyDefinitions(std::string prefix) :
+ ModuleBasePropertyDefinitions(prefix)
{
- defineOptionalProperty<float>(
- "SelfCollisionCheck_Frequency", 10,
- "Frequency [Hz] of self collision checking (default = 10). If set to 0, no cllisions will be checked.",
- PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<float>(
- "SelfCollisionCheck_MinSelfDistance", 20,
- "Minimum allowed distance (mm) between each collision model before entering emergency stop (default = 20).",
- PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>("SelfCollisionCheck_Frequency",
+ 10,
+ "Frequency [Hz] of self collision checking (default = "
+ "10). If set to 0, no cllisions will be checked.",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>("SelfCollisionCheck_MinSelfDistance",
+ 20,
+ "Minimum allowed distance (mm) between each collision "
+ "model before entering emergency stop (default = 20).",
+ PropertyDefinitionBase::eModifiable);
defineOptionalProperty<std::string>(
- "SelfCollisionCheck_ModelGroupsToCheck", "",
- "Comma seperated list of groups (two or more) of collision models (RobotNodeSets or RobotNodes) that "
+ "SelfCollisionCheck_ModelGroupsToCheck",
+ "",
+ "Comma seperated list of groups (two or more) of collision models (RobotNodeSets "
+ "or RobotNodes) that "
"should be checked against each other by collision avoidance \n"
"(e.g. {rnsColModel1,rnsColModel2};{rnsColModel3,rnsColModel4}; .... "
- "says that rnsColModel1 will be only checked against rnsColModel2 and rnsColModel3 will only be checked against rnsColModel4). \n"
- "Following predefined descriptors can be used: 'FLOOR' which represents a virtual collision model of the floor.");
+ "says that rnsColModel1 will be only checked against rnsColModel2 and rnsColModel3 "
+ "will only be checked against rnsColModel4). \n"
+ "Following predefined descriptors can be used: 'FLOOR' which represents a virtual "
+ "collision model of the floor.");
}
};
@@ -63,18 +71,23 @@ namespace armarx::RobotUnitModule
*
* @see ModuleBase
*/
- class SelfCollisionChecker : virtual public ModuleBase, virtual public RobotUnitSelfCollisionCheckerInterface
+ class SelfCollisionChecker :
+ virtual public ModuleBase,
+ virtual public RobotUnitSelfCollisionCheckerInterface
{
friend class ModuleBase;
+
public:
/**
* @brief Returns the singleton instance of this class
* @return The singleton instance of this class
*/
- static SelfCollisionChecker& Instance()
+ static SelfCollisionChecker&
+ Instance()
{
return ModuleBase::Instance<SelfCollisionChecker>();
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -95,12 +108,14 @@ namespace armarx::RobotUnitModule
* @brief Sets the minimal distance (mm) between a configured pair of bodies to 'distance'.
* @param distance The minimal distance (mm) between a pair of bodies.
*/
- void setSelfCollisionAvoidanceDistance(Ice::Float distance, const Ice::Current& = Ice::emptyCurrent) override;
+ void setSelfCollisionAvoidanceDistance(Ice::Float distance,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Sets the frequency of self collision checks.
* @param freq The frequency of self collision checks.
*/
- void setSelfCollisionAvoidanceFrequency(Ice::Float freq, const Ice::Current& = Ice::emptyCurrent) override;
+ void setSelfCollisionAvoidanceFrequency(Ice::Float freq,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Returns whether the frequency of self collision checks is above 0.
@@ -111,12 +126,14 @@ namespace armarx::RobotUnitModule
* @brief Returns the frequency of self collision checks.
* @return The frequency of self collision checks.
*/
- float getSelfCollisionAvoidanceFrequency(const Ice::Current& = Ice::emptyCurrent) const override;
+ float
+ getSelfCollisionAvoidanceFrequency(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the minimal distance (mm) between a pair of bodies.
* @return The minimal distance (mm) between a pair of bodies.
*/
- float getSelfCollisionAvoidanceDistance(const Ice::Current& = Ice::emptyCurrent) const override;
+ float
+ getSelfCollisionAvoidanceDistance(const Ice::Current& = Ice::emptyCurrent) const override;
// //////////////////////////////////////////////////////////////////////////////////////// //
// //////////////////////////////////// implementation //////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -134,9 +151,9 @@ namespace armarx::RobotUnitModule
// //////////////////////////////////////////////////////////////////////////////////////// //
private:
/// @brief The list of pairs of collision models that should be checked agaisnt each other (names only)
- std::set< std::pair<std::string, std::string>> namePairsToCheck;
+ std::set<std::pair<std::string, std::string>> namePairsToCheck;
/// @brief The frequency of self collision checks.
- std::atomic<float> checkFrequency {0};
+ std::atomic<float> checkFrequency{0};
// //////////////////////////////////////////////////////////////////////////////////////// //
// /////////////////////////////////////// col data /////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -144,9 +161,10 @@ namespace armarx::RobotUnitModule
///@ brief The mutex guarding data used for self collision checks.
std::mutex selfCollisionDataMutex;
/// @brief Min allowed distance (mm) between each pair of collision models
- std::atomic<float> minSelfDistance {0};
+ std::atomic<float> minSelfDistance{0};
/// @brief The list of pairs of collision models that should be checked agaisnt each other (model nodes only)
- std::vector<std::pair<VirtualRobot::SceneObjectPtr, VirtualRobot::SceneObjectPtr>> nodePairsToCheck;
+ std::vector<std::pair<VirtualRobot::SceneObjectPtr, VirtualRobot::SceneObjectPtr>>
+ nodePairsToCheck;
/// @brief The Robot used for self collison checks
VirtualRobot::RobotPtr selfCollisionAvoidanceRobot;
/// @brief The Robot Nodes of the Robot used for collision checks
@@ -154,6 +172,6 @@ namespace armarx::RobotUnitModule
/// @brief A Collision object for the floor
VirtualRobot::SceneObjectSetPtr floor;
/// @brief The pair of nodes causing the last detected collision (as index in \ref nodePairsToCheck or -1)
- std::atomic_size_t lastCollisionPairIndex {std::numeric_limits<std::size_t>::max()};
+ std::atomic_size_t lastCollisionPairIndex{std::numeric_limits<std::size_t>::max()};
};
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index 80efce08aeaefed56bf599ceb928c7e1615e36a0..ad3cc0f97680eabc2519b58598823294c770733f 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -20,15 +20,17 @@
* GNU General Public License
*/
-#include <ArmarXCore/core/IceManager.h>
+#include "RobotUnitModuleUnits.h"
+
#include <ArmarXCore/core/IceGridAdmin.h>
+#include <ArmarXCore/core/IceManager.h>
#include "RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h"
#include "RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h"
#include <RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h>
-#include "RobotUnitModuleUnits.h"
-
+#include "../RobotUnit.h"
+#include "../SensorValues/SensorValue1DoFActuator.h"
#include "../Units/ForceTorqueSubUnit.h"
#include "../Units/InertialMeasurementSubUnit.h"
#include "../Units/KinematicSubUnit.h"
@@ -36,10 +38,6 @@
#include "../Units/TCPControllerSubUnit.h"
#include "../Units/TrajectoryControllerSubUnit.h"
-#include "../SensorValues/SensorValue1DoFActuator.h"
-
-#include "../RobotUnit.h"
-
namespace armarx::RobotUnitModule
{
/**
@@ -49,12 +47,15 @@ namespace armarx::RobotUnitModule
class ControlThreadAttorneyForRobotUnitEmergencyStopMaster
{
friend class RobotUnitEmergencyStopMaster;
- static void SetEmergencyStopStateNoReportToTopic(ControlThread* controlThreadModule, EmergencyStopState state)
+
+ static void
+ SetEmergencyStopStateNoReportToTopic(ControlThread* controlThreadModule,
+ EmergencyStopState state)
{
controlThreadModule->setEmergencyStopStateNoReportToTopic(state);
}
};
-}
+} // namespace armarx::RobotUnitModule
namespace armarx::RobotUnitModule
{
@@ -63,40 +64,50 @@ namespace armarx::RobotUnitModule
virtual public EmergencyStopMasterInterface
{
public:
- RobotUnitEmergencyStopMaster(ControlThread* controlThreadModule, std::string emergencyStopTopicName) :
- controlThreadModule {controlThreadModule},
- emergencyStopTopicName {emergencyStopTopicName}
+ RobotUnitEmergencyStopMaster(ControlThread* controlThreadModule,
+ std::string emergencyStopTopicName) :
+ controlThreadModule{controlThreadModule}, emergencyStopTopicName{emergencyStopTopicName}
{
ARMARX_CHECK_NOT_NULL(controlThreadModule);
ARMARX_CHECK_EXPRESSION(!emergencyStopTopicName.empty());
}
- void setEmergencyStopState(EmergencyStopState state, const Ice::Current& = Ice::emptyCurrent) final override
+ void
+ setEmergencyStopState(EmergencyStopState state,
+ const Ice::Current& = Ice::emptyCurrent) final override
{
if (getEmergencyStopState() == state)
{
return;
}
- ControlThreadAttorneyForRobotUnitEmergencyStopMaster::SetEmergencyStopStateNoReportToTopic(controlThreadModule, state);
+ ControlThreadAttorneyForRobotUnitEmergencyStopMaster::
+ SetEmergencyStopStateNoReportToTopic(controlThreadModule, state);
emergencyStopTopic->reportEmergencyStopState(state);
}
- EmergencyStopState getEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const final override
+
+ EmergencyStopState
+ getEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const final override
{
return controlThreadModule->getEmergencyStopState();
}
protected:
- void onInitComponent() final override
+ void
+ onInitComponent() final override
{
armarx::ManagedIceObject::offeringTopic(emergencyStopTopicName);
}
- void onConnectComponent() final override
+
+ void
+ onConnectComponent() final override
{
- emergencyStopTopic = armarx::ManagedIceObject::getTopic<EmergencyStopListenerPrx>(emergencyStopTopicName);
+ emergencyStopTopic = armarx::ManagedIceObject::getTopic<EmergencyStopListenerPrx>(
+ emergencyStopTopicName);
setEmergencyStopState(EmergencyStopState::eEmergencyStopInactive);
}
- std::string getDefaultName() const final override
+ std::string
+ getDefaultName() const final override
{
return "EmergencyStopMaster";
}
@@ -106,11 +117,12 @@ namespace armarx::RobotUnitModule
const std::string emergencyStopTopicName;
EmergencyStopListenerPrx emergencyStopTopic;
};
-}
+} // namespace armarx::RobotUnitModule
namespace armarx::RobotUnitModule
{
- Ice::ObjectProxySeq Units::getUnits(const Ice::Current&) const
+ Ice::ObjectProxySeq
+ Units::getUnits(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::vector<ManagedIceObjectPtr> unitsCopy;
@@ -128,7 +140,8 @@ namespace armarx::RobotUnitModule
return r;
}
- Ice::ObjectPrx Units::getUnit(const std::string& staticIceId, const Ice::Current&) const
+ Ice::ObjectPrx
+ Units::getUnit(const std::string& staticIceId, const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
//no lock required
@@ -140,13 +153,15 @@ namespace armarx::RobotUnitModule
return nullptr;
}
- const EmergencyStopMasterInterfacePtr& Units::getEmergencyStopMaster() const
+ const EmergencyStopMasterInterfacePtr&
+ Units::getEmergencyStopMaster() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return emergencyStopMaster;
}
- void Units::initializeDefaultUnits()
+ void
+ Units::initializeDefaultUnits()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto beg = TimeUtil::GetTime(true);
@@ -171,12 +186,13 @@ namespace armarx::RobotUnitModule
ARMARX_DEBUG << "TrajectoryControllerUnit initialized";
initializeTcpControllerUnit();
ARMARX_DEBUG << "TcpControllerUnit initialized";
-
}
- ARMARX_INFO << "initializing default units...done! " << (TimeUtil::GetTime(true) - beg).toMicroSeconds() << " us";
+ ARMARX_INFO << "initializing default units...done! "
+ << (TimeUtil::GetTime(true) - beg).toMicroSeconds() << " us";
}
- void Units::initializeKinematicUnit()
+ void
+ Units::initializeKinematicUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
using IfaceT = KinematicUnitInterface;
@@ -196,13 +212,12 @@ namespace armarx::RobotUnitModule
}
}
- ManagedIceObjectPtr Units::createKinematicSubUnit(
- const Ice::PropertiesPtr& properties,
- const std::string& positionControlMode,
- const std::string& velocityControlMode,
- const std::string& torqueControlMode,
- const std::string& pwmControlMode
- )
+ ManagedIceObjectPtr
+ Units::createKinematicSubUnit(const Ice::PropertiesPtr& properties,
+ const std::string& positionControlMode,
+ const std::string& velocityControlMode,
+ const std::string& torqueControlMode,
+ const std::string& pwmControlMode)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -212,7 +227,8 @@ namespace armarx::RobotUnitModule
//add ctrl et al
ARMARX_DEBUG << "createKinematicSubUnit...";
std::map<std::string, UnitT::ActuatorData> devs;
- for (const ControlDevicePtr& controlDevice : _module<Devices>().getControlDevices().values())
+ for (const ControlDevicePtr& controlDevice :
+ _module<Devices>().getControlDevices().values())
{
ARMARX_CHECK_NOT_NULL(controlDevice);
const auto& controlDeviceName = controlDevice->getDeviceName();
@@ -222,19 +238,21 @@ namespace armarx::RobotUnitModule
{
UnitT::ActuatorData ad;
ad.name = controlDeviceName;
- ARMARX_DEBUG << "has sensor device: " << _module<Devices>().getSensorDevices().has(controlDeviceName);
+ ARMARX_DEBUG << "has sensor device: "
+ << _module<Devices>().getSensorDevices().has(controlDeviceName);
ad.sensorDeviceIndex =
- _module<Devices>().getSensorDevices().has(controlDeviceName) ?
- _module<Devices>().getSensorDevices().index(controlDeviceName) :
- std::numeric_limits<std::size_t>::max();
+ _module<Devices>().getSensorDevices().has(controlDeviceName)
+ ? _module<Devices>().getSensorDevices().index(controlDeviceName)
+ : std::numeric_limits<std::size_t>::max();
- const auto init_controller = [&](const std::string & requestedControlMode, auto & ctrl, const auto * tpptr)
+ const auto init_controller =
+ [&](const std::string& requestedControlMode, auto& ctrl, const auto* tpptr)
{
auto controlMode = requestedControlMode;
using CtargT = std::remove_const_t<std::remove_reference_t<decltype(*tpptr)>>;
NJointKinematicUnitPassThroughControllerPtr& pt = ctrl;
- auto testMode = [&](const auto & m)
+ auto testMode = [&](const auto& m)
{
JointController* jointCtrl = controlDevice->getJointController(m);
return jointCtrl && jointCtrl->getControlTarget()->isA<CtargT>();
@@ -254,8 +272,7 @@ namespace armarx::RobotUnitModule
{
selected_ctrl = nullptr;
ARMARX_INFO << "Autodetected two controllers supporting "
- << requestedControlMode
- << "! autoselection failed";
+ << requestedControlMode << "! autoselection failed";
break;
}
selected_ctrl = ctrl;
@@ -264,42 +281,49 @@ namespace armarx::RobotUnitModule
if (selected_ctrl)
{
controlMode = selected_ctrl->getControlMode();
- ARMARX_INFO << "Autodetected controller with mode "
- << controlMode << "(the requested mode was "
- << requestedControlMode << ")";
+ ARMARX_INFO << "Autodetected controller with mode " << controlMode
+ << "(the requested mode was " << requestedControlMode
+ << ")";
}
}
if (!controlMode.empty())
{
- NJointKinematicUnitPassThroughControllerConfigPtr config = new NJointKinematicUnitPassThroughControllerConfig;
+ NJointKinematicUnitPassThroughControllerConfigPtr config =
+ new NJointKinematicUnitPassThroughControllerConfig;
config->controlMode = controlMode;
config->deviceName = controlDeviceName;
- auto ctrl_name = getName() + "_" + "NJointKU_PTCtrl_" + controlDeviceName + "_" + controlMode;
+ auto ctrl_name = getName() + "_" + "NJointKU_PTCtrl_" + controlDeviceName +
+ "_" + controlMode;
std::replace(ctrl_name.begin(), ctrl_name.end(), '.', '_');
std::replace(ctrl_name.begin(), ctrl_name.end(), '-', '_');
std::replace(ctrl_name.begin(), ctrl_name.end(), ':', '_');
- const NJointControllerBasePtr& nJointCtrl = _module<ControllerManagement>().createNJointController(
- "NJointKinematicUnitPassThroughController",
- ctrl_name, config, false, true);
+ const NJointControllerBasePtr& nJointCtrl =
+ _module<ControllerManagement>().createNJointController(
+ "NJointKinematicUnitPassThroughController",
+ ctrl_name,
+ config,
+ false,
+ true);
pt = NJointKinematicUnitPassThroughControllerPtr::dynamicCast(nJointCtrl);
ARMARX_CHECK_EXPRESSION(pt);
}
};
- init_controller(positionControlMode, ad.ctrlPos, (ControlTarget1DoFActuatorPosition*)0);
- init_controller(velocityControlMode, ad.ctrlVel, (ControlTarget1DoFActuatorVelocity*)0);
- init_controller(torqueControlMode, ad.ctrlTor, (ControlTarget1DoFActuatorTorque*)0);
+ init_controller(
+ positionControlMode, ad.ctrlPos, (ControlTarget1DoFActuatorPosition*)0);
+ init_controller(
+ velocityControlMode, ad.ctrlVel, (ControlTarget1DoFActuatorVelocity*)0);
+ init_controller(torqueControlMode, ad.ctrlTor, (ControlTarget1DoFActuatorTorque*)0);
if (!ad.ctrlTor)
{
- init_controller(pwmControlMode, ad.ctrlTor, (ControlTarget1DoFActuatorPWM*)0);
+ init_controller(pwmControlMode, ad.ctrlTor, (ControlTarget1DoFActuatorPWM*)0);
}
if (ad.ctrlPos || ad.ctrlTor || ad.ctrlVel)
{
- ARMARX_DEBUG << "created controllers for "
- << controlDeviceName
- << " pos/tor/vel " << ad.ctrlPos.get() << " / "
- << ad.ctrlTor.get() << " / " << ad.ctrlVel.get();
+ ARMARX_DEBUG << "created controllers for " << controlDeviceName
+ << " pos/tor/vel " << ad.ctrlPos.get() << " / " << ad.ctrlTor.get()
+ << " / " << ad.ctrlVel.get();
devs[controlDeviceName] = std::move(ad);
}
}
@@ -310,33 +334,38 @@ namespace armarx::RobotUnitModule
return nullptr;
}
ARMARX_IMPORTANT << "Found " << devs.size() << " joint devices - adding KinematicUnit";
- ARMARX_CHECK_EXPRESSION(!devs.empty()) << "no 1DoF ControlDevice found with matching SensorDevice";
+ ARMARX_CHECK_EXPRESSION(!devs.empty())
+ << "no 1DoF ControlDevice found with matching SensorDevice";
//add it
const std::string configName = getProperty<std::string>("KinematicUnitName");
const std::string confPre = getConfigDomain() + "." + configName + ".";
//properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
//fill properties
- properties->setProperty(confPre + "RobotNodeSetName", _module<RobotData>().getRobotNodetSeName());
+ properties->setProperty(confPre + "RobotNodeSetName",
+ _module<RobotData>().getRobotNodetSeName());
properties->setProperty(confPre + "RobotFileName", _module<RobotData>().getRobotFileName());
- properties->setProperty(confPre + "RobotFileNameProject", _module<RobotData>().getRobotProjectName());
- properties->setProperty(confPre + "TopicPrefix", getProperty<std::string>("KinematicUnitNameTopicPrefix"));
+ properties->setProperty(confPre + "RobotFileNameProject",
+ _module<RobotData>().getRobotProjectName());
+ properties->setProperty(confPre + "TopicPrefix",
+ getProperty<std::string>("KinematicUnitNameTopicPrefix"));
- ARMARX_DEBUG << "creating unit " << configName << " using these properties: " << properties->getPropertiesForPrefix("");
- IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
+ ARMARX_DEBUG << "creating unit " << configName
+ << " using these properties: " << properties->getPropertiesForPrefix("");
+ IceInternal::Handle<UnitT> unit =
+ Component::create<UnitT>(properties, configName, getConfigDomain());
//fill devices (sensor + controller)
- unit->setupData(
- getProperty<std::string>("RobotFileName").getValue(),
- _module<RobotData>().cloneRobot(),
- std::move(devs),
- _module<Devices>().getControlModeGroups().groups,
- _module<Devices>().getControlModeGroups().groupsMerged,
- dynamic_cast<RobotUnit*>(this)
- );
+ unit->setupData(getProperty<std::string>("RobotFileName").getValue(),
+ _module<RobotData>().cloneRobot(),
+ std::move(devs),
+ _module<Devices>().getControlModeGroups().groups,
+ _module<Devices>().getControlModeGroups().groupsMerged,
+ dynamic_cast<RobotUnit*>(this));
return unit;
}
- void Units::initializePlatformUnit()
+ void
+ Units::initializePlatformUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
using UnitT = PlatformSubUnit;
@@ -355,76 +384,91 @@ namespace armarx::RobotUnitModule
ARMARX_INFO << "no platform unit created since no platform name was given";
return;
}
- if (!_module<Devices>().getControlDevices().has(_module<RobotData>().getRobotPlatformName()))
+ if (!_module<Devices>().getControlDevices().has(
+ _module<RobotData>().getRobotPlatformName()))
{
- ARMARX_WARNING << "no platform unit created since the platform control device with name '"
- << _module<RobotData>().getRobotPlatformName() << "' was not found";
+ ARMARX_WARNING
+ << "no platform unit created since the platform control device with name '"
+ << _module<RobotData>().getRobotPlatformName() << "' was not found";
return;
}
- const ControlDevicePtr& controlDevice = _module<Devices>().getControlDevices().at(_module<RobotData>().getRobotPlatformName());
- const SensorDevicePtr& sensorDevice = _module<Devices>().getSensorDevices().at(_module<RobotData>().getRobotPlatformName());
- JointController* jointVel = controlDevice->getJointController(ControlModes::HolonomicPlatformVelocity);
+ const ControlDevicePtr& controlDevice =
+ _module<Devices>().getControlDevices().at(_module<RobotData>().getRobotPlatformName());
+ const SensorDevicePtr& sensorDevice =
+ _module<Devices>().getSensorDevices().at(_module<RobotData>().getRobotPlatformName());
+ JointController* jointVel =
+ controlDevice->getJointController(ControlModes::HolonomicPlatformVelocity);
ARMARX_CHECK_EXPRESSION(jointVel);
- ARMARX_CHECK_EXPRESSION(sensorDevice->getSensorValue()->asA<SensorValueHolonomicPlatform>());
+ ARMARX_CHECK_EXPRESSION(
+ sensorDevice->getSensorValue()->asA<SensorValueHolonomicPlatform>());
//add it
const std::string configName = getProperty<std::string>("PlatformUnitName");
const std::string confPre = getConfigDomain() + "." + configName + ".";
Ice::PropertiesPtr properties = getIceProperties()->clone();
//properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
//fill properties
- properties->setProperty(confPre + "PlatformName", _module<RobotData>().getRobotPlatformInstanceName());
- ARMARX_DEBUG << "creating unit " << configName << " using these properties: " << properties->getPropertiesForPrefix("");
- IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
+ properties->setProperty(confPre + "PlatformName",
+ _module<RobotData>().getRobotPlatformInstanceName());
+ ARMARX_DEBUG << "creating unit " << configName
+ << " using these properties: " << properties->getPropertiesForPrefix("");
+ IceInternal::Handle<UnitT> unit =
+ Component::create<UnitT>(properties, configName, getConfigDomain());
//config
- NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr config = new NJointHolonomicPlatformUnitVelocityPassThroughControllerConfig;
+ NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr config =
+ new NJointHolonomicPlatformUnitVelocityPassThroughControllerConfig;
config->initialVelocityX = 0;
config->initialVelocityY = 0;
config->initialVelocityRotation = 0;
config->platformName = _module<RobotData>().getRobotPlatformName();
auto ctrl = NJointHolonomicPlatformUnitVelocityPassThroughControllerPtr::dynamicCast(
- _module<ControllerManagement>().createNJointController(
- "NJointHolonomicPlatformUnitVelocityPassThroughController",
- getName() + "_" + configName + "_VelPTContoller",
- config, false, true
- )
- );
+ _module<ControllerManagement>().createNJointController(
+ "NJointHolonomicPlatformUnitVelocityPassThroughController",
+ getName() + "_" + configName + "_VelPTContoller",
+ config,
+ false,
+ true));
ARMARX_CHECK_EXPRESSION(ctrl);
unit->pt = ctrl;
- NJointHolonomicPlatformRelativePositionControllerConfigPtr configRelativePositionCtrlCfg = new NJointHolonomicPlatformRelativePositionControllerConfig;
+ NJointHolonomicPlatformRelativePositionControllerConfigPtr configRelativePositionCtrlCfg =
+ new NJointHolonomicPlatformRelativePositionControllerConfig;
configRelativePositionCtrlCfg->platformName = _module<RobotData>().getRobotPlatformName();
- auto ctrlRelativePosition = NJointHolonomicPlatformRelativePositionControllerPtr::dynamicCast(
- _module<ControllerManagement>().createNJointController(
- "NJointHolonomicPlatformRelativePositionController",
- getName() + "_" + configName + "_RelativePositionContoller",
- configRelativePositionCtrlCfg, false, true
- )
- );
+ auto ctrlRelativePosition =
+ NJointHolonomicPlatformRelativePositionControllerPtr::dynamicCast(
+ _module<ControllerManagement>().createNJointController(
+ "NJointHolonomicPlatformRelativePositionController",
+ getName() + "_" + configName + "_RelativePositionContoller",
+ configRelativePositionCtrlCfg,
+ false,
+ true));
ARMARX_CHECK_EXPRESSION(ctrlRelativePosition);
unit->pt = ctrl;
unit->relativePosCtrl = ctrlRelativePosition;
// unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(_module<RobotData>().getRobotPlatformName());
- NJointHolonomicPlatformGlobalPositionControllerConfigPtr configGlobalPositionCtrlCfg = new NJointHolonomicPlatformGlobalPositionControllerConfig;
+ NJointHolonomicPlatformGlobalPositionControllerConfigPtr configGlobalPositionCtrlCfg =
+ new NJointHolonomicPlatformGlobalPositionControllerConfig;
configGlobalPositionCtrlCfg->platformName = _module<RobotData>().getRobotPlatformName();
auto ctrlGlobalPosition = NJointHolonomicPlatformGlobalPositionControllerPtr::dynamicCast(
- _module<ControllerManagement>().createNJointController(
- "NJointHolonomicPlatformGlobalPositionController",
- getName() + "_" + configName + "_GlobalPositionContoller",
- configGlobalPositionCtrlCfg, false, true
- )
- );
+ _module<ControllerManagement>().createNJointController(
+ "NJointHolonomicPlatformGlobalPositionController",
+ getName() + "_" + configName + "_GlobalPositionContoller",
+ configGlobalPositionCtrlCfg,
+ false,
+ true));
ARMARX_CHECK_EXPRESSION(ctrlGlobalPosition);
unit->pt = ctrl;
unit->globalPosCtrl = ctrlGlobalPosition;
- unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(_module<RobotData>().getRobotPlatformName());
+ unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(
+ _module<RobotData>().getRobotPlatformName());
//add
addUnit(std::move(unit));
}
- void Units::initializeLocalizationUnit()
+ void
+ Units::initializeLocalizationUnit()
{
ARMARX_DEBUG << "initializeLocalizationUnit";
@@ -441,8 +485,10 @@ namespace armarx::RobotUnitModule
}
ARMARX_DEBUG << "Getting device SensorValueHolonomicPlatformRelativePosition";
- const SensorDevicePtr& sensorDeviceRelativePosition = _module<Devices>().getSensorDevices().at(_module<RobotData>().getRobotPlatformName());
- ARMARX_CHECK_EXPRESSION(sensorDeviceRelativePosition->getSensorValue()->asA<SensorValueHolonomicPlatformRelativePosition>());
+ const SensorDevicePtr& sensorDeviceRelativePosition =
+ _module<Devices>().getSensorDevices().at(_module<RobotData>().getRobotPlatformName());
+ ARMARX_CHECK_EXPRESSION(sensorDeviceRelativePosition->getSensorValue()
+ ->asA<SensorValueHolonomicPlatformRelativePosition>());
// const SensorDevicePtr& sensorDevicePoseCorrection = _module<Devices>().getSensorDevices().at(GlobalRobotPoseCorrectionSensorDevice::DeviceName());
// ARMARX_CHECK_EXPRESSION(sensorDeviceRelativePosition->getSensorValue()->asA<SensorValueGlobalPoseCorrection>());
@@ -455,26 +501,41 @@ namespace armarx::RobotUnitModule
//properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
//fill properties
// properties->setProperty(confPre + "PlatformName", _module<RobotData>().getRobotPlatformInstanceName());
- ARMARX_DEBUG << "creating unit " << configName << " using these properties: " << properties->getPropertiesForPrefix("");
- IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
+ ARMARX_DEBUG << "creating unit " << configName
+ << " using these properties: " << properties->getPropertiesForPrefix("");
+ IceInternal::Handle<UnitT> unit =
+ Component::create<UnitT>(properties, configName, getConfigDomain());
//config
ARMARX_DEBUG << "Setting device GlobalRobotPoseCorrectionSensorDevice";
- unit->globalPositionCorrectionSensorDevice = dynamic_cast<decltype(unit->globalPositionCorrectionSensorDevice)>(
- _module<Devices>().getSensorDevices().at(GlobalRobotPoseCorrectionSensorDevice::DeviceName()).get());
-
-
- const SensorDevicePtr& sensorDeviceGlobalRobotLocalization = _module<Devices>().getSensorDevices().at(GlobalRobotLocalizationSensorDevice::DeviceName());
- ARMARX_CHECK_EXPRESSION(sensorDeviceGlobalRobotLocalization->getSensorValue()->asA<GlobalRobotLocalizationSensorDevice::SensorValueType>());
-
- dynamic_cast<GlobalRobotLocalizationSensorDevice&>(*sensorDeviceGlobalRobotLocalization).sensorRelativePosition = sensorDeviceRelativePosition->getSensorValue()->asA<SensorValueHolonomicPlatformRelativePosition>();
- dynamic_cast<GlobalRobotLocalizationSensorDevice&>(*sensorDeviceGlobalRobotLocalization).sensorGlobalPositionCorrection = unit->globalPositionCorrectionSensorDevice->getSensorValue()->asA<SensorValueGlobalPoseCorrection>();
+ unit->globalPositionCorrectionSensorDevice =
+ dynamic_cast<decltype(unit->globalPositionCorrectionSensorDevice)>(
+ _module<Devices>()
+ .getSensorDevices()
+ .at(GlobalRobotPoseCorrectionSensorDevice::DeviceName())
+ .get());
+
+
+ const SensorDevicePtr& sensorDeviceGlobalRobotLocalization =
+ _module<Devices>().getSensorDevices().at(
+ GlobalRobotLocalizationSensorDevice::DeviceName());
+ ARMARX_CHECK_EXPRESSION(sensorDeviceGlobalRobotLocalization->getSensorValue()
+ ->asA<GlobalRobotLocalizationSensorDevice::SensorValueType>());
+
+ dynamic_cast<GlobalRobotLocalizationSensorDevice&>(*sensorDeviceGlobalRobotLocalization)
+ .sensorRelativePosition = sensorDeviceRelativePosition->getSensorValue()
+ ->asA<SensorValueHolonomicPlatformRelativePosition>();
+ dynamic_cast<GlobalRobotLocalizationSensorDevice&>(*sensorDeviceGlobalRobotLocalization)
+ .sensorGlobalPositionCorrection =
+ unit->globalPositionCorrectionSensorDevice->getSensorValue()
+ ->asA<SensorValueGlobalPoseCorrection>();
//add
addUnit(unit);
}
- void Units::initializeForceTorqueUnit()
+ void
+ Units::initializeForceTorqueUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
using UnitT = ForceTorqueSubUnit;
@@ -494,24 +555,28 @@ namespace armarx::RobotUnitModule
if (ftSensorDevice->getSensorValue()->isA<SensorValueForceTorque>())
{
ForceTorqueSubUnit::DeviceData ftSensorData;
- ftSensorData.sensorIndex = _module<Devices>().getSensorDevices().index(ftSensorDevice->getDeviceName());
+ ftSensorData.sensorIndex =
+ _module<Devices>().getSensorDevices().index(ftSensorDevice->getDeviceName());
ftSensorData.deviceName = ftSensorDevice->getDeviceName();
ftSensorData.frame = ftSensorDevice->getReportingFrame();
- ARMARX_CHECK_EXPRESSION(
- !ftSensorData.frame.empty()) <<
- "The sensor device '" << ftSensorData.deviceName << "' (index: " << ftSensorData.sensorIndex
- << ") reports force torque values but returns an empty string as reporting frame";
- ARMARX_CHECK_EXPRESSION(
- unitCreateRobot->hasRobotNode(ftSensorData.frame)) <<
- "The sensor device '" << ftSensorData.deviceName << "' (index: " << ftSensorData.sensorIndex
- << ") reports force torque values but returns a reporting frame '" << ftSensorData.frame
- << "' which is not present in the robot '" << _module<RobotData>().getRobotName() << "'";
+ ARMARX_CHECK_EXPRESSION(!ftSensorData.frame.empty())
+ << "The sensor device '" << ftSensorData.deviceName
+ << "' (index: " << ftSensorData.sensorIndex
+ << ") reports force torque values but returns an empty string as reporting "
+ "frame";
+ ARMARX_CHECK_EXPRESSION(unitCreateRobot->hasRobotNode(ftSensorData.frame))
+ << "The sensor device '" << ftSensorData.deviceName
+ << "' (index: " << ftSensorData.sensorIndex
+ << ") reports force torque values but returns a reporting frame '"
+ << ftSensorData.frame << "' which is not present in the robot '"
+ << _module<RobotData>().getRobotName() << "'";
ftdevs.emplace_back(std::move(ftSensorData));
}
}
if (ftdevs.empty())
{
- ARMARX_IMPORTANT << "no force torque unit created since there are no force torque sensor devices";
+ ARMARX_IMPORTANT
+ << "no force torque unit created since there are no force torque sensor devices";
return;
}
//add it
@@ -521,15 +586,19 @@ namespace armarx::RobotUnitModule
//properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
//fill properties
properties->setProperty(confPre + "AgentName", _module<RobotData>().getRobotName());
- properties->setProperty(confPre + "ForceTorqueTopicName", getProperty<std::string>("ForceTorqueTopicName").getValue());
- ARMARX_DEBUG << "creating unit " << configName << " using these properties: " << properties->getPropertiesForPrefix("");
- IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
+ properties->setProperty(confPre + "ForceTorqueTopicName",
+ getProperty<std::string>("ForceTorqueTopicName").getValue());
+ ARMARX_DEBUG << "creating unit " << configName
+ << " using these properties: " << properties->getPropertiesForPrefix("");
+ IceInternal::Handle<UnitT> unit =
+ Component::create<UnitT>(properties, configName, getConfigDomain());
unit->devs = ftdevs;
//add
addUnit(std::move(unit));
}
- void Units::initializeInertialMeasurementUnit()
+ void
+ Units::initializeInertialMeasurementUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
using UnitT = InertialMeasurementSubUnit;
@@ -554,7 +623,8 @@ namespace armarx::RobotUnitModule
}
if (imudevs.empty())
{
- ARMARX_IMPORTANT << "no inertial measurement unit created since there are no imu sensor devices";
+ ARMARX_IMPORTANT
+ << "no inertial measurement unit created since there are no imu sensor devices";
return;
}
//add it
@@ -563,14 +633,17 @@ namespace armarx::RobotUnitModule
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, getConfigDomain());
+ properties->setProperty(confPre + "IMUTopicName",
+ getProperty<std::string>("IMUTopicName").getValue());
+ IceInternal::Handle<UnitT> unit =
+ Component::create<UnitT>(properties, configName, getConfigDomain());
unit->devs = imudevs;
//add
addUnit(std::move(unit));
}
- void Units::initializeTrajectoryControllerUnit()
+ void
+ Units::initializeTrajectoryControllerUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
@@ -586,12 +659,14 @@ namespace armarx::RobotUnitModule
{
return;
}
- (TrajectoryControllerSubUnitPtr::dynamicCast(trajectoryControllerSubUnit))->setup(_modulePtr<RobotUnit>());
+ (TrajectoryControllerSubUnitPtr::dynamicCast(trajectoryControllerSubUnit))
+ ->setup(_modulePtr<RobotUnit>());
addUnit(trajectoryControllerSubUnit);
trajectoryControllerSubUnit = nullptr;
}
- void Units::initializeTcpControllerUnit()
+ void
+ Units::initializeTcpControllerUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
@@ -608,12 +683,14 @@ namespace armarx::RobotUnitModule
{
return;
}
- (TCPControllerSubUnitPtr::dynamicCast(tcpControllerSubUnit))->setup(_modulePtr<RobotUnit>(), _module<RobotData>().cloneRobot());
+ (TCPControllerSubUnitPtr::dynamicCast(tcpControllerSubUnit))
+ ->setup(_modulePtr<RobotUnit>(), _module<RobotData>().cloneRobot());
addUnit(tcpControllerSubUnit);
tcpControllerSubUnit = nullptr;
}
- void Units::addUnit(ManagedIceObjectPtr unit)
+ void
+ Units::addUnit(ManagedIceObjectPtr unit)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_CHECK_NOT_NULL(unit);
@@ -629,7 +706,8 @@ namespace armarx::RobotUnitModule
units.emplace_back(std::move(unit));
}
- void Units::_icePropertiesInitialized()
+ void
+ Units::_icePropertiesInitialized()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
Ice::PropertiesPtr properties = getIceProperties()->clone();
@@ -637,23 +715,31 @@ namespace armarx::RobotUnitModule
// create TCPControlSubUnit
{
- const std::string configNameTCPControlUnit = getProperty<std::string>("TCPControlUnitName");
- tcpControllerSubUnit = Component::create<TCPControllerSubUnit, TCPControllerSubUnitPtr>(properties, configNameTCPControlUnit, configDomain);
+ const std::string configNameTCPControlUnit =
+ getProperty<std::string>("TCPControlUnitName");
+ tcpControllerSubUnit = Component::create<TCPControllerSubUnit, TCPControllerSubUnitPtr>(
+ properties, configNameTCPControlUnit, configDomain);
addPropertyUser(PropertyUserPtr::dynamicCast(tcpControllerSubUnit));
}
// create TrajectoryControllerSubUnit
{
- const std::string configNameTrajectoryControllerUnit = getProperty<std::string>("TrajectoryControllerUnitName");
- const std::string confPre = configDomain + "." + configNameTrajectoryControllerUnit + ".";
- properties->setProperty(confPre + "KinematicUnitName", getProperty<std::string>("KinematicUnitName").getValue());
- trajectoryControllerSubUnit = Component::create<TrajectoryControllerSubUnit, TrajectoryControllerSubUnitPtr>(properties, configNameTrajectoryControllerUnit, configDomain);
+ const std::string configNameTrajectoryControllerUnit =
+ getProperty<std::string>("TrajectoryControllerUnitName");
+ const std::string confPre =
+ configDomain + "." + configNameTrajectoryControllerUnit + ".";
+ properties->setProperty(confPre + "KinematicUnitName",
+ getProperty<std::string>("KinematicUnitName").getValue());
+ trajectoryControllerSubUnit =
+ Component::create<TrajectoryControllerSubUnit, TrajectoryControllerSubUnitPtr>(
+ properties, configNameTrajectoryControllerUnit, configDomain);
addPropertyUser(PropertyUserPtr::dynamicCast(trajectoryControllerSubUnit));
}
}
- void Units::_preFinishRunning()
+ void
+ Units::_preFinishRunning()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
//remove all units
@@ -665,47 +751,61 @@ namespace armarx::RobotUnitModule
units.clear();
}
- void Units::_preOnInitRobotUnit()
+ void
+ Units::_preOnInitRobotUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
unitCreateRobot = _module<RobotData>().cloneRobot();
ARMARX_DEBUG << "add emergency stop master";
{
- emergencyStopMaster = new RobotUnitEmergencyStopMaster {&_module<ControlThread>(), getProperty<std::string>("EmergencyStopTopic").getValue()};
+ emergencyStopMaster = new RobotUnitEmergencyStopMaster{
+ &_module<ControlThread>(),
+ getProperty<std::string>("EmergencyStopTopic").getValue()};
ManagedIceObjectPtr obj = ManagedIceObjectPtr::dynamicCast(emergencyStopMaster);
ARMARX_CHECK_EXPRESSION(obj) << "RobotUnitEmergencyStopMaster";
- getArmarXManager()->addObject(obj, getProperty<std::string>("EmergencyStopMasterName").getValue(), true);
+ getArmarXManager()->addObject(
+ obj, getProperty<std::string>("EmergencyStopMasterName").getValue(), true);
auto prx = obj->getProxy(-1);
try
{
- getArmarXManager()->getIceManager()->getIceGridSession()->getAdmin()->addObject(prx);
+ getArmarXManager()->getIceManager()->getIceGridSession()->getAdmin()->addObject(
+ prx);
}
catch (const IceGrid::ObjectExistsException&)
{
- getArmarXManager()->getIceManager()->getIceGridSession()->getAdmin()->updateObject(prx);
+ getArmarXManager()->getIceManager()->getIceGridSession()->getAdmin()->updateObject(
+ prx);
}
catch (std::exception& e)
{
- ARMARX_WARNING << "add the robot unit as emergency stop master to the ice grid admin!\nThere was an exception:\n" << e.what();
+ ARMARX_WARNING << "add the robot unit as emergency stop master to the ice grid "
+ "admin!\nThere was an exception:\n"
+ << e.what();
}
}
ARMARX_DEBUG << "add emergency stop master...done!";
}
- void Units::_postOnExitRobotUnit()
+ void
+ Units::_postOnExitRobotUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_DEBUG << "remove EmergencyStopMaster";
try
{
- getArmarXManager()->removeObjectBlocking(ManagedIceObjectPtr::dynamicCast(emergencyStopMaster));
- getArmarXManager()->getIceManager()->removeObject(getProperty<std::string>("EmergencyStopMasterName").getValue());
+ getArmarXManager()->removeObjectBlocking(
+ ManagedIceObjectPtr::dynamicCast(emergencyStopMaster));
+ getArmarXManager()->getIceManager()->removeObject(
+ getProperty<std::string>("EmergencyStopMasterName").getValue());
+ }
+ catch (...)
+ {
}
- catch (...) {}
ARMARX_DEBUG << "remove EmergencyStopMaster...done!";
}
- const ManagedIceObjectPtr& Units::getUnit(const std::string& staticIceId) const
+ const ManagedIceObjectPtr&
+ Units::getUnit(const std::string& staticIceId) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
@@ -718,4 +818,4 @@ namespace armarx::RobotUnitModule
}
return ManagedIceObject::NullPtr;
}
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
index ecb6e49b5e3ebd44fb358e976b7ffdfe68d56a82..6a423722690a411ab270477bcacd387118ba8d61 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
@@ -24,61 +24,64 @@
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-#include "RobotUnitModuleBase.h"
#include "../Units/RobotUnitSubUnit.h"
+#include "RobotUnitModuleBase.h"
namespace armarx::RobotUnitModule
{
- class UnitsPropertyDefinitions: public ModuleBasePropertyDefinitions
+ class UnitsPropertyDefinitions : public ModuleBasePropertyDefinitions
{
public:
- UnitsPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
+ UnitsPropertyDefinitions(std::string prefix) : ModuleBasePropertyDefinitions(prefix)
{
defineOptionalProperty<std::string>(
- "KinematicUnitName", "KinematicUnit",
- "The name of the created kinematic unit");
+ "KinematicUnitName", "KinematicUnit", "The name of the created kinematic unit");
defineOptionalProperty<std::string>(
- "KinematicUnitNameTopicPrefix", "",
- "Prefix for the kinematic sensor values topic");
+ "KinematicUnitNameTopicPrefix", "", "Prefix for the kinematic sensor values topic");
defineOptionalProperty<std::string>(
- "PlatformUnitName", "PlatformUnit",
- "The name of the created platform unit");
+ "PlatformUnitName", "PlatformUnit", "The name of the created platform unit");
defineOptionalProperty<std::string>(
- "ForceTorqueUnitName", "ForceTorqueUnit",
+ "ForceTorqueUnitName",
+ "ForceTorqueUnit",
"The name of the created force troque unit (empty = default)");
defineOptionalProperty<std::string>(
- "ForceTorqueTopicName", "ForceTorqueValues",
+ "ForceTorqueTopicName",
+ "ForceTorqueValues",
"Name of the topic on which the force torque sensor values are provided");
defineOptionalProperty<std::string>(
- "InertialMeasurementUnitName", "InertialMeasurementUnit",
+ "InertialMeasurementUnitName",
+ "InertialMeasurementUnit",
"The name of the created inertial measurement unit (empty = default)");
defineOptionalProperty<std::string>(
- "IMUTopicName", "IMUValues",
- "Name of the IMU Topic.");
+ "IMUTopicName", "IMUValues", "Name of the IMU Topic.");
- defineOptionalProperty<bool>(
- "CreateTCPControlUnit", false,
- "If true the TCPControl SubUnit is created and added");
+ defineOptionalProperty<bool>("CreateTCPControlUnit",
+ false,
+ "If true the TCPControl SubUnit is created and added");
defineOptionalProperty<std::string>(
- "TCPControlUnitName", "TCPControlUnit",
- "Name of the TCPControlUnit.");
+ "TCPControlUnitName", "TCPControlUnit", "Name of the TCPControlUnit.");
defineOptionalProperty<bool>(
- "CreateTrajectoryPlayer", true,
+ "CreateTrajectoryPlayer",
+ true,
"If true the TrajectoryPlayer SubUnit is created and added");
defineOptionalProperty<std::string>(
- "TrajectoryControllerUnitName", "TrajectoryPlayer",
- "Name of the TrajectoryControllerUnit. The respective component outside of the RobotUnit is TrajectoryPlayer");
+ "TrajectoryControllerUnitName",
+ "TrajectoryPlayer",
+ "Name of the TrajectoryControllerUnit. The respective component outside of the "
+ "RobotUnit is TrajectoryPlayer");
defineOptionalProperty<std::string>(
- "EmergencyStopMasterName", "EmergencyStopMaster",
+ "EmergencyStopMasterName",
+ "EmergencyStopMaster",
"The name used to register as an EmergencyStopMaster");
defineOptionalProperty<std::string>(
- "EmergencyStopTopic", "EmergencyStop",
+ "EmergencyStopTopic",
+ "EmergencyStop",
"The name of the topic over which changes of the emergencyStopState are sent.");
}
};
@@ -100,6 +103,7 @@ namespace armarx::RobotUnitModule
class Units : virtual public ModuleBase, virtual public RobotUnitUnitInterface
{
friend class ModuleBase;
+
public:
/**
* @brief Returns the singleton instance of this class
@@ -148,7 +152,8 @@ namespace armarx::RobotUnitModule
* @brief Returns a proxy to the InertialMeasurementUnit
* @return A proxy to the InertialMeasurementUnit
*/
- InertialMeasurementUnitInterfacePrx getInertialMeasurementUnit(const Ice::Current&) const override;
+ InertialMeasurementUnitInterfacePrx
+ getInertialMeasurementUnit(const Ice::Current&) const override;
/**
* @brief Returns a proxy to the PlatformUnit
* @return A proxy to the PlatformUnit
@@ -180,12 +185,14 @@ namespace armarx::RobotUnitModule
* @brief Returns a pointer to the Unit for the given type (or null if there is none)
* @return A pointer to the Unit for the given type (or null if there is none)
*/
- template<class T> typename T::PointerType getUnit() const;
+ template <class T>
+ typename T::PointerType getUnit() const;
/**
* @brief Returns a proxy to the Unit for the given type (or null if there is none)
* @return A proxy to the Unit for the given type (or null if there is none)
*/
- template<class T> typename T::ProxyType getUnitPrx() const;
+ template <class T>
+ typename T::ProxyType getUnitPrx() const;
//specific getters
/**
@@ -254,11 +261,12 @@ namespace armarx::RobotUnitModule
/// @brief Initializes the TcpControllerUnit
virtual void initializeLocalizationUnit();
/// @brief Create the \ref KinematicUnit (this function should be called in \ref initializeKinematicUnit)
- ManagedIceObjectPtr createKinematicSubUnit(const Ice::PropertiesPtr& properties,
- const std::string& positionControlMode = ControlModes::Position1DoF,
- const std::string& velocityControlMode = ControlModes::Velocity1DoF,
- const std::string& torqueControlMode = ControlModes::Torque1DoF,
- const std::string& pwmControlMode = ControlModes::PWM1DoF);
+ ManagedIceObjectPtr
+ createKinematicSubUnit(const Ice::PropertiesPtr& properties,
+ const std::string& positionControlMode = ControlModes::Position1DoF,
+ const std::string& velocityControlMode = ControlModes::Velocity1DoF,
+ const std::string& torqueControlMode = ControlModes::Torque1DoF,
+ const std::string& pwmControlMode = ControlModes::PWM1DoF);
/**
* @brief Registers a unit for management (the Unit is added and removed to the \ref ArmarXManager
* and updated with new values.
@@ -295,6 +303,6 @@ namespace armarx::RobotUnitModule
*/
friend class UnitsAttorneyForPublisher;
};
-}
+} // namespace armarx::RobotUnitModule
#include "RobotUnitModuleUnits.ipp"
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModules.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModules.h
index 75c57423496559a2a7cca4437f67e0ab3c0542bd..4df7d8cc88aca99131d3587df0443054dd7a2084 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModules.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModules.h
@@ -23,14 +23,13 @@
#pragma once
#include "RobotUnitModuleBase.h"
-
-#include "RobotUnitModuleDevices.h"
-#include "RobotUnitModuleControlThreadDataBuffer.h"
#include "RobotUnitModuleControlThread.h"
-#include "RobotUnitModuleUnits.h"
+#include "RobotUnitModuleControlThreadDataBuffer.h"
+#include "RobotUnitModuleControllerManagement.h"
+#include "RobotUnitModuleDevices.h"
#include "RobotUnitModuleLogging.h"
+#include "RobotUnitModuleManagement.h"
#include "RobotUnitModulePublisher.h"
#include "RobotUnitModuleRobotData.h"
-#include "RobotUnitModuleControllerManagement.h"
-#include "RobotUnitModuleManagement.h"
#include "RobotUnitModuleSelfCollisionChecker.h"
+#include "RobotUnitModuleUnits.h"
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.cpp
index 004d3361faf19b3acfdfd99e0278d0246c004449..2af079acbfd5e4bfdd9459de758055b44bcdf641 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.cpp
@@ -4,19 +4,22 @@
namespace armarx
{
- void RobotUnitObserver::offerOrUpdateDataFieldsFlatCopy_async(const std::string& channelName, StringVariantBaseMap&& valueMap)
+ void
+ RobotUnitObserver::offerOrUpdateDataFieldsFlatCopy_async(const std::string& channelName,
+ StringVariantBaseMap&& valueMap)
{
- addWorkerJob("RobotUnitObserver::offerOrUpdateDataFieldsFlatCopy_async", [this, channelName, vmap = std::move(valueMap)]
- {
- offerOrUpdateDataFieldsFlatCopy(channelName, vmap);
- });
+ addWorkerJob("RobotUnitObserver::offerOrUpdateDataFieldsFlatCopy_async",
+ [this, channelName, vmap = std::move(valueMap)]
+ { offerOrUpdateDataFieldsFlatCopy(channelName, vmap); });
}
- void RobotUnitObserver::onConnectObserver()
+ void
+ RobotUnitObserver::onConnectObserver()
{
offerChannel(sensorDevicesChannel, "Devices that provide sensor data");
offerChannel(controlDevicesChannel, "Devices that are controlled by JointControllers");
- offerChannel(timingChannel, "Channel for timings of the different phases of the robot unit");
+ offerChannel(timingChannel,
+ "Channel for timings of the different phases of the robot unit");
offerChannel(additionalChannel, "Additional custom datafields");
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.h
index 2fdfb990e824b343cb88337c8316363ceb964a3e..36a127da9f45974c4ad7083f7bd2ca3ce00c87ca 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.h
@@ -23,19 +23,25 @@ namespace armarx
RobotUnitObserver() = default;
- void offerOrUpdateDataFieldsFlatCopy_async(const std::string& channelName, StringVariantBaseMap&& valueMap);
+ void offerOrUpdateDataFieldsFlatCopy_async(const std::string& channelName,
+ StringVariantBaseMap&& valueMap);
// Observer interface
protected:
- void onInitObserver() override {}
+ void
+ onInitObserver() override
+ {
+ }
+
void onConnectObserver() override;
friend class RobotUnitModule::Publisher;
// ManagedIceObject interface
protected:
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "RobotUnitObserver";
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
index cdb365742dad36bc50872c5ba86b3a3e58be9d87..354424245a27f6b086dbff417a25fd3c2e510124 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
@@ -21,25 +21,25 @@
*/
#pragma once
-#include "SensorValueBase.h"
-
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include "SensorValueBase.h"
+
namespace armarx
{
-#define make_SensorValue1DoFActuator(type, name, varname) \
- class name : virtual public SensorValueBase \
- { \
- public: \
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION \
- type varname = 0.0f; \
- static SensorValueInfo<name> GetClassMemberInfo() \
- { \
- SensorValueInfo<name> svi; \
- svi.addMemberVariable(&name::varname, #varname); \
- return svi; \
- } \
+#define make_SensorValue1DoFActuator(type, name, varname) \
+ class name : virtual public SensorValueBase \
+ { \
+ public: \
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION type varname = 0.0f; \
+ static SensorValueInfo<name> \
+ GetClassMemberInfo() \
+ { \
+ SensorValueInfo<name> svi; \
+ svi.addMemberVariable(&name::varname, #varname); \
+ return svi; \
+ } \
}
make_SensorValue1DoFActuator(float, SensorValue1DoFActuatorPosition, position);
@@ -54,24 +54,25 @@ namespace armarx
#undef make_SensorValue1DoFActuator
- class SensorValue1DoFInverseDynamicsTorque :
- virtual public SensorValue1DoFGravityTorque
+ class SensorValue1DoFInverseDynamicsTorque : virtual public SensorValue1DoFGravityTorque
{
public:
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- float inverseDynamicsTorque = 0.0f;
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION float inverseDynamicsTorque = 0.0f;
float inertiaTorque = 0.0f;
- static SensorValueInfo<SensorValue1DoFInverseDynamicsTorque> GetClassMemberInfo()
+
+ static SensorValueInfo<SensorValue1DoFInverseDynamicsTorque>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValue1DoFInverseDynamicsTorque> svi;
svi.addBaseClass<SensorValue1DoFGravityTorque>();
- svi.addMemberVariable(&SensorValue1DoFInverseDynamicsTorque::inverseDynamicsTorque, "inverseDynamicsTorque");
- svi.addMemberVariable(&SensorValue1DoFInverseDynamicsTorque::inertiaTorque, "inertiaTorque");
+ svi.addMemberVariable(&SensorValue1DoFInverseDynamicsTorque::inverseDynamicsTorque,
+ "inverseDynamicsTorque");
+ svi.addMemberVariable(&SensorValue1DoFInverseDynamicsTorque::inertiaTorque,
+ "inertiaTorque");
return svi;
}
};
-
class SensorValue1DoFActuator :
virtual public SensorValue1DoFActuatorPosition,
virtual public SensorValue1DoFActuatorVelocity,
@@ -80,8 +81,9 @@ namespace armarx
virtual public SensorValue1DoFInverseDynamicsTorque
{
public:
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- static SensorValueInfo<SensorValue1DoFActuator> GetClassMemberInfo()
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION static SensorValueInfo<
+ SensorValue1DoFActuator>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValue1DoFActuator> svi;
svi.addBaseClass<SensorValue1DoFActuatorPosition>();
@@ -96,13 +98,17 @@ namespace armarx
class SensorValue1DoFActuatorStatus : virtual public SensorValueBase
{
public:
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- armarx::JointStatus status;
- static SensorValueInfo<SensorValue1DoFActuatorStatus> GetClassMemberInfo()
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION armarx::JointStatus status;
+
+ static SensorValueInfo<SensorValue1DoFActuatorStatus>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValue1DoFActuatorStatus> svi;
svi.addMemberVariable(&SensorValue1DoFActuatorStatus::status, "JointStatus")
- .setFieldNames({"JointStatusError", "JointStatusOperation", "JointStatusEnabled", "JointStatusEmergencyStop"});
+ .setFieldNames({"JointStatusError",
+ "JointStatusOperation",
+ "JointStatusEnabled",
+ "JointStatusEmergencyStop"});
return svi;
}
};
@@ -113,8 +119,9 @@ namespace armarx
virtual public SensorValue1DoFActuatorMotorTemperature
{
public:
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- static SensorValueInfo<SensorValue1DoFRealActuator> GetClassMemberInfo()
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION static SensorValueInfo<
+ SensorValue1DoFRealActuator>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValue1DoFRealActuator> svi;
svi.addBaseClass<SensorValue1DoFActuator>();
@@ -129,8 +136,9 @@ namespace armarx
virtual public SensorValue1DoFActuatorStatus
{
public:
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- static SensorValueInfo<SensorValue1DoFRealActuatorWithStatus> GetClassMemberInfo()
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION static SensorValueInfo<
+ SensorValue1DoFRealActuatorWithStatus>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValue1DoFRealActuatorWithStatus> svi;
svi.addBaseClass<SensorValue1DoFRealActuator>();
@@ -138,4 +146,4 @@ namespace armarx
return svi;
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h
index 021ab483d56dc0536e28004102fafaacd629677f..8a9dc1e258078bb856cd4c6596bc67c5ff046b65 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h
@@ -21,15 +21,15 @@
*/
#pragma once
-#include <typeinfo>
+#include <map>
#include <memory>
#include <string>
-#include <map>
+#include <typeinfo>
#include <ArmarXCore/util/CPPUtility/TemplateMetaProgramming.h>
-#include <RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h>
#include <RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainerMacros.h>
+#include <RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h>
namespace armarx
{
@@ -39,151 +39,165 @@ namespace armarx
*/
class SensorValueBase
{
- template<class...Ts>
+ template <class... Ts>
struct IsAHelper
{
// only called then sizeof...(Ts) == 0
static_assert(sizeof...(Ts) == 0, "called overload for empty pack with params");
- static bool IsA(const SensorValueBase* sv)
+
+ static bool
+ IsA(const SensorValueBase* sv)
{
return true;
}
};
- template<class T, class...Ts>
+
+ template <class T, class... Ts>
struct IsAHelper<T, Ts...>
{
- static bool IsA(const SensorValueBase* sv)
+ static bool
+ IsA(const SensorValueBase* sv)
{
return dynamic_cast<const T*>(sv) && IsAHelper<Ts...>::IsA(sv);
}
};
public:
- template<class DerivedClass>
+ template <class DerivedClass>
using SensorValueInfo = introspection::ClassMemberInfo<SensorValueBase, DerivedClass>;
virtual ~SensorValueBase() = default;
virtual std::string getSensorValueType(bool withoutNamespaceSpecifier) const = 0;
- template<class...Ts>
- bool isA() const
+ template <class... Ts>
+ bool
+ isA() const
{
return IsAHelper<Ts...>::IsA(this);
}
- template<class T>
- const T* asA() const
+ template <class T>
+ const T*
+ asA() const
{
return dynamic_cast<const T*>(this);
}
- template<class T>
- T* asA()
+ template <class T>
+ T*
+ asA()
{
return dynamic_cast<T*>(this);
}
//logging functions
/// @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 = 0;
+ virtual std::map<std::string, VariantBasePtr>
+ toVariants(const IceUtil::Time& timestamp) const = 0;
virtual std::size_t getNumberOfDataFields() const = 0;
virtual std::vector<std::string> getDataFieldNames() const = 0;
- virtual void getDataFieldAs(std::size_t i, bool& out) const = 0;
- virtual void getDataFieldAs(std::size_t i, Ice::Byte& out) const = 0;
- virtual void getDataFieldAs(std::size_t i, Ice::Short& out) const = 0;
- virtual void getDataFieldAs(std::size_t i, Ice::Int& out) const = 0;
- virtual void getDataFieldAs(std::size_t i, Ice::Long& out) const = 0;
- virtual void getDataFieldAs(std::size_t i, Ice::Float& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, bool& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Byte& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Short& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Int& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Long& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Float& out) const = 0;
virtual void getDataFieldAs(std::size_t i, Ice::Double& out) const = 0;
virtual void getDataFieldAs(std::size_t i, std::string& out) const = 0;
- template<class T>
- T getDataFieldAs(std::size_t i) const
+
+ template <class T>
+ T
+ getDataFieldAs(std::size_t i) const
{
ARMARX_TRACE;
T t;
this->getDataFieldAs(i, t);
return t;
}
+
virtual const std::type_info& getDataFieldType(std::size_t i) const = 0;
//management functions
- template<class T, class = typename std::enable_if<std::is_base_of<SensorValueBase, T>::value>::type>
- void _copyTo(std::unique_ptr<T>& target) const
+ 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());
}
- ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(
- SensorValueHasGetClassMemberInfo,
- GetClassMemberInfo, SensorValueInfo<T>(*)(void));
+ ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(SensorValueHasGetClassMemberInfo,
+ GetClassMemberInfo,
+ SensorValueInfo<T> (*)(void));
ARMARX_PLACEMENT_CONSTRUCTION_HELPER_BASE(SensorValueBase)
};
-}
-
-#define DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION \
- ARMARX_PLACEMENT_CONSTRUCTION_HELPER \
- using SensorValueBase = ::armarx::SensorValueBase; \
- using VariantBasePtr = ::armarx::VariantBasePtr; \
- std::string getSensorValueType(bool withoutNamespaceSpecifier = false) const override \
- { \
- return armarx::GetTypeString(*this, withoutNamespaceSpecifier); \
- } \
- void _check_for_static_GetClassMemberInfo_overload() \
- { \
- static_assert(SensorValueHasGetClassMemberInfo<std::decay<decltype(*this)>::type>::value, \
- "This class has to implement GetClassMemberInfo() returning " \
- "an instance of SensorValueInfo<THIS_CLASS_TYPE>"); \
- } \
- std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override \
- { \
- return SensorValueInfo<std::decay<decltype(*this)>::type>::ToVariants(timestamp,this); \
- } \
- std::size_t getNumberOfDataFields() const override \
- { \
- return SensorValueInfo<std::decay<decltype(*this)>::type>::GetNumberOfDataFields(); \
- } \
- void getDataFieldAs (std::size_t i, bool& out) const override \
- { \
- SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Byte& out) const override \
- { \
- SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Short& out) const override \
- { \
- SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Int& out) const override \
- { \
- SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Long& out) const override \
- { \
- SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Float& out) const override \
- { \
- SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Double& out) const override \
- { \
- SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, std::string& out) const override \
- { \
- SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- const std::type_info& getDataFieldType(std::size_t i) const override \
- { \
- return SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldType(i); \
- } \
- std::vector<std::string> getDataFieldNames() const override \
- { \
- return SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldNames(); \
+} // namespace armarx
+
+#define DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION \
+ ARMARX_PLACEMENT_CONSTRUCTION_HELPER \
+ using SensorValueBase = ::armarx::SensorValueBase; \
+ using VariantBasePtr = ::armarx::VariantBasePtr; \
+ std::string getSensorValueType(bool withoutNamespaceSpecifier = false) const override \
+ { \
+ return armarx::GetTypeString(*this, withoutNamespaceSpecifier); \
+ } \
+ void _check_for_static_GetClassMemberInfo_overload() \
+ { \
+ static_assert(SensorValueHasGetClassMemberInfo<std::decay<decltype(*this)>::type>::value, \
+ "This class has to implement GetClassMemberInfo() returning " \
+ "an instance of SensorValueInfo<THIS_CLASS_TYPE>"); \
+ } \
+ std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) \
+ const override \
+ { \
+ return SensorValueInfo<std::decay<decltype(*this)>::type>::ToVariants(timestamp, this); \
+ } \
+ std::size_t getNumberOfDataFields() const override \
+ { \
+ return SensorValueInfo<std::decay<decltype(*this)>::type>::GetNumberOfDataFields(); \
+ } \
+ void getDataFieldAs(std::size_t i, bool& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Byte& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Short& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Int& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Long& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Float& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Double& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, std::string& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ const std::type_info& getDataFieldType(std::size_t i) const override \
+ { \
+ return SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldType(i); \
+ } \
+ std::vector<std::string> getDataFieldNames() const override \
+ { \
+ return SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldNames(); \
}
namespace armarx
@@ -191,10 +205,11 @@ namespace armarx
class SensorValueDummy : public SensorValueBase
{
public:
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- static SensorValueInfo<SensorValueDummy> GetClassMemberInfo()
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION static SensorValueInfo<
+ SensorValueDummy>
+ GetClassMemberInfo()
{
- return SensorValueInfo<SensorValueDummy> {};
+ return SensorValueInfo<SensorValueDummy>{};
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h
index 68f41cb91bc10b7744706c452d71de9eb1093014..776bb731d7ab5115953fb6cb044c0f385ce6e7a2 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h
@@ -21,31 +21,36 @@
*/
#pragma once
+#include <RobotAPI/libraries/core/Pose.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;
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION Eigen::Vector3f torque;
Eigen::Vector3f force;
Eigen::Vector3f gravityCompensatedTorque;
Eigen::Vector3f gravityCompensatedForce;
- static SensorValueInfo<SensorValueForceTorque> GetClassMemberInfo()
+
+ static SensorValueInfo<SensorValueForceTorque>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValueForceTorque> svi;
- svi.addMemberVariable(&SensorValueForceTorque::torque, "torque").setFieldNames({"tx", "ty", "tz"});
- svi.addMemberVariable(&SensorValueForceTorque::force, "force").setFieldNames({"fx", "fy", "fz"});
- svi.addMemberVariable(&SensorValueForceTorque::gravityCompensatedTorque, "gravityCompensatedTorque")
- .setFieldNames({"gravCompTx", "gravCompTy", "gravCompTz"});
- svi.addMemberVariable(&SensorValueForceTorque::gravityCompensatedForce, "gravityCompensatedForce")
- .setFieldNames({"gravCompFx", "gravCompFy", "gravCompFz"});
+ svi.addMemberVariable(&SensorValueForceTorque::torque, "torque")
+ .setFieldNames({"tx", "ty", "tz"});
+ svi.addMemberVariable(&SensorValueForceTorque::force, "force")
+ .setFieldNames({"fx", "fy", "fz"});
+ svi.addMemberVariable(&SensorValueForceTorque::gravityCompensatedTorque,
+ "gravityCompensatedTorque")
+ .setFieldNames({"gravCompTx", "gravCompTy", "gravCompTz"});
+ svi.addMemberVariable(&SensorValueForceTorque::gravityCompensatedForce,
+ "gravityCompensatedForce")
+ .setFieldNames({"gravCompFx", "gravCompFy", "gravCompFz"});
return svi;
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h
index b7318e72fe1ae0cc7811389a0eccaa95f80a5ef5..53d921fccbd6ef14dd113f7c442be549a6a6e053 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h
@@ -21,25 +21,27 @@
*/
#pragma once
-#include "SensorValueBase.h"
-
#include <ArmarXCore/core/util/algorithm.h>
+#include "SensorValueBase.h"
+
namespace armarx
{
class SensorValueHolonomicPlatformVelocity : virtual public SensorValueBase
{
public:
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- float velocityX;
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION float velocityX;
float velocityY;
float velocityRotation;
- static SensorValueInfo<SensorValueHolonomicPlatformVelocity> GetClassMemberInfo()
+
+ static SensorValueInfo<SensorValueHolonomicPlatformVelocity>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValueHolonomicPlatformVelocity> svi;
svi.addMemberVariable(&SensorValueHolonomicPlatformVelocity::velocityX, "velocityX");
svi.addMemberVariable(&SensorValueHolonomicPlatformVelocity::velocityY, "velocityY");
- svi.addMemberVariable(&SensorValueHolonomicPlatformVelocity::velocityRotation, "velocityRotation");
+ svi.addMemberVariable(&SensorValueHolonomicPlatformVelocity::velocityRotation,
+ "velocityRotation");
return svi;
}
};
@@ -47,22 +49,28 @@ namespace armarx
class SensorValueHolonomicPlatformAcceleration : virtual public SensorValueBase
{
public:
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- float accelerationX;
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION float accelerationX;
float accelerationY;
float accelerationRotation;
- void deriveAccelerationFromVelocityDelta(float dvx, float dvy, float dvrot, float dt)
+
+ void
+ deriveAccelerationFromVelocityDelta(float dvx, float dvy, float dvrot, float dt)
{
accelerationX = dvx / dt;
accelerationY = dvy / dt;
accelerationRotation = dvrot / dt;
}
- static SensorValueInfo<SensorValueHolonomicPlatformAcceleration> GetClassMemberInfo()
+
+ static SensorValueInfo<SensorValueHolonomicPlatformAcceleration>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValueHolonomicPlatformAcceleration> svi;
- svi.addMemberVariable(&SensorValueHolonomicPlatformAcceleration::accelerationX, "accelerationX");
- svi.addMemberVariable(&SensorValueHolonomicPlatformAcceleration::accelerationY, "accelerationY");
- svi.addMemberVariable(&SensorValueHolonomicPlatformAcceleration::accelerationRotation, "accelerationRotation");
+ svi.addMemberVariable(&SensorValueHolonomicPlatformAcceleration::accelerationX,
+ "accelerationX");
+ svi.addMemberVariable(&SensorValueHolonomicPlatformAcceleration::accelerationY,
+ "accelerationY");
+ svi.addMemberVariable(&SensorValueHolonomicPlatformAcceleration::accelerationRotation,
+ "accelerationRotation");
return svi;
}
};
@@ -77,22 +85,26 @@ namespace armarx
public:
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- float relativePositionX = 0;
+ float relativePositionX = 0;
float relativePositionY = 0;
float relativePositionRotation = 0;
- static SensorValueInfo<SensorValueHolonomicPlatformRelativePosition> GetClassMemberInfo()
+ static SensorValueInfo<SensorValueHolonomicPlatformRelativePosition>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValueHolonomicPlatformRelativePosition> svi;
- svi.addMemberVariable(&SensorValueHolonomicPlatformRelativePosition::relativePositionX, "relativePositionX");
- svi.addMemberVariable(&SensorValueHolonomicPlatformRelativePosition::relativePositionY, "relativePositionY");
- svi.addMemberVariable(&SensorValueHolonomicPlatformRelativePosition::relativePositionRotation, "relativePositionRotation");
+ svi.addMemberVariable(&SensorValueHolonomicPlatformRelativePosition::relativePositionX,
+ "relativePositionX");
+ svi.addMemberVariable(&SensorValueHolonomicPlatformRelativePosition::relativePositionY,
+ "relativePositionY");
+ svi.addMemberVariable(
+ &SensorValueHolonomicPlatformRelativePosition::relativePositionRotation,
+ "relativePositionRotation");
return svi;
}
};
-
- // The following class is deprecated: if the absolute position is provided, it will become inconsistent with the relative position.
+ // The following class is deprecated: if the absolute position is provided, it will become inconsistent with the relative position.
// Therefore, SensorValueHolonomicPlatformRelativePosition + SensorValueHolonomicPlatformGlobalPositionCorrection should be used instead.
// class SensorValueHolonomicPlatformAbsolutePosition : virtual public SensorValueBase
@@ -112,7 +124,6 @@ namespace armarx
// }
// };
-
class SensorValueHolonomicPlatform :
virtual public SensorValueHolonomicPlatformVelocity,
@@ -122,14 +133,18 @@ namespace armarx
public:
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- void setVelocitiesAndDeriveAcceleration(float vx, float vy, float vrot, float dt)
+ void
+ setVelocitiesAndDeriveAcceleration(float vx, float vy, float vrot, float dt)
{
- deriveAccelerationFromVelocityDelta(vx - velocityX, vy - velocityY, vrot - velocityRotation, dt);
+ deriveAccelerationFromVelocityDelta(
+ vx - velocityX, vy - velocityY, vrot - velocityRotation, dt);
velocityX = vx;
velocityY = vy;
velocityRotation = vrot;
}
- static SensorValueInfo<SensorValueHolonomicPlatform> GetClassMemberInfo()
+
+ static SensorValueInfo<SensorValueHolonomicPlatform>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValueHolonomicPlatform> svi;
svi.addBaseClass<SensorValueHolonomicPlatformVelocity>();
@@ -139,7 +154,7 @@ namespace armarx
}
};
- // The following class is deprecated: if the absolute position is provided, it will become inconsistent with the relative position.
+ // The following class is deprecated: if the absolute position is provided, it will become inconsistent with the relative position.
// Therefore, SensorValueHolonomicPlatformRelativePosition + SensorValueHolonomicPlatformGlobalPositionCorrection should be used instead.
// class SensorValueHolonomicPlatformWithAbsolutePosition : virtual public SensorValueHolonomicPlatform, virtual public SensorValueHolonomicPlatformAbsolutePosition
@@ -154,4 +169,4 @@ namespace armarx
// return svi;
// }
// };
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueIMU.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueIMU.h
index d8e596482f17f042f7a8d89355915565891cc88c..51080678e35f25dfaa69c619193a05b7a18efb37 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueIMU.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueIMU.h
@@ -24,23 +24,24 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
-#include "SensorValueBase.h"
-
#include <ArmarXCore/core/util/algorithm.h>
+
#include <RobotAPI/libraries/core/Pose.h>
+#include "SensorValueBase.h"
+
namespace armarx
{
- class SensorValueIMU :
- virtual public SensorValueBase
+ class SensorValueIMU : virtual public SensorValueBase
{
public:
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- Eigen::Vector3f angularVelocity = Eigen::Vector3f::Zero();
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION Eigen::Vector3f angularVelocity =
+ Eigen::Vector3f::Zero();
Eigen::Vector3f linearAcceleration = Eigen::Vector3f::Zero();
Eigen::Quaternionf orientation = Eigen::Quaternionf::Identity();
- static SensorValueInfo<SensorValueIMU> GetClassMemberInfo()
+ static SensorValueInfo<SensorValueIMU>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValueIMU> svi;
svi.addMemberVariable(&SensorValueIMU::angularVelocity, "angularVelocity");
@@ -49,4 +50,4 @@ namespace armarx
return svi;
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueRTThreadTimings.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueRTThreadTimings.h
index ece2aa54cdd1681f3f1f4f1887b624cb4040cf3b..2f13e7cc6a665999dbfd90ba3a0563e372fb4c6e 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueRTThreadTimings.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueRTThreadTimings.h
@@ -21,12 +21,12 @@
*/
#pragma once
-#include "SensorValueBase.h"
-
#include <chrono>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
+#include "SensorValueBase.h"
+
namespace armarx
{
class SensorValueRTThreadTimings : public SensorValueBase
@@ -34,7 +34,7 @@ namespace armarx
public:
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- IceUtil::Time rtSwitchControllerSetupDuration;
+ IceUtil::Time rtSwitchControllerSetupDuration;
IceUtil::Time rtSwitchControllerSetupRoundTripTime;
IceUtil::Time rtRunNJointControllersDuration;
@@ -61,28 +61,49 @@ namespace armarx
IceUtil::Time rtLoopDuration;
IceUtil::Time rtLoopRoundTripTime;
- static SensorValueInfo<SensorValueRTThreadTimings> GetClassMemberInfo()
+ static SensorValueInfo<SensorValueRTThreadTimings>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValueRTThreadTimings> svi;
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtSwitchControllerSetupDuration, "rtSwitchControllerSetup.Duration");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtSwitchControllerSetupRoundTripTime, "rtSwitchControllerSetup.RoundTripTime");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtRunNJointControllersDuration, "rtRunNJointControllers.Duration");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtRunNJointControllersRoundTripTime, "rtRunNJointControllers.RoundTripTime");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtHandleInvalidTargetsDuration, "rtHandleInvalidTargets.Duration");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtHandleInvalidTargetsRoundTripTime, "rtHandleInvalidTargets.RoundTripTime");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtRunJointControllersDuration, "rtRunJointControllers.Duration");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtRunJointControllersRoundTripTime, "rtRunJointControllers.RoundTripTime");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtBusSendReceiveDuration, "rtBusSendReceive.Duration");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtBusSendReceiveRoundTripTime, "rtBusSendReceive.RoundTripTime");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtReadSensorDeviceValuesDuration, "rtReadSensorDeviceValues.Duration");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtReadSensorDeviceValuesRoundTripTime, "rtReadSensorDeviceValues.RoundTripTime");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtUpdateSensorAndControlBufferDuration, "rtUpdateSensorAndControlBuffer.Duration");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtUpdateSensorAndControlBufferRoundTripTime, "rtUpdateSensorAndControlBuffer.RoundTripTime");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtResetAllTargetsDuration, "rtResetAllTargets.Duration");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtResetAllTargetsRoundTripTime, "rtResetAllTargets.RoundTripTime");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtLoopDuration, "rtLoop.Duration");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtLoopRoundTripTime, "rtLoop.RoundTripTime");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtSwitchControllerSetupDuration,
+ "rtSwitchControllerSetup.Duration");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtSwitchControllerSetupRoundTripTime,
+ "rtSwitchControllerSetup.RoundTripTime");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtRunNJointControllersDuration,
+ "rtRunNJointControllers.Duration");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtRunNJointControllersRoundTripTime,
+ "rtRunNJointControllers.RoundTripTime");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtHandleInvalidTargetsDuration,
+ "rtHandleInvalidTargets.Duration");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtHandleInvalidTargetsRoundTripTime,
+ "rtHandleInvalidTargets.RoundTripTime");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtRunJointControllersDuration,
+ "rtRunJointControllers.Duration");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtRunJointControllersRoundTripTime,
+ "rtRunJointControllers.RoundTripTime");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtBusSendReceiveDuration,
+ "rtBusSendReceive.Duration");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtBusSendReceiveRoundTripTime,
+ "rtBusSendReceive.RoundTripTime");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtReadSensorDeviceValuesDuration,
+ "rtReadSensorDeviceValues.Duration");
+ svi.addMemberVariable(
+ &SensorValueRTThreadTimings::rtReadSensorDeviceValuesRoundTripTime,
+ "rtReadSensorDeviceValues.RoundTripTime");
+ svi.addMemberVariable(
+ &SensorValueRTThreadTimings::rtUpdateSensorAndControlBufferDuration,
+ "rtUpdateSensorAndControlBuffer.Duration");
+ svi.addMemberVariable(
+ &SensorValueRTThreadTimings::rtUpdateSensorAndControlBufferRoundTripTime,
+ "rtUpdateSensorAndControlBuffer.RoundTripTime");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtResetAllTargetsDuration,
+ "rtResetAllTargets.Duration");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtResetAllTargetsRoundTripTime,
+ "rtResetAllTargets.RoundTripTime");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtLoopDuration, "rtLoop.Duration");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtLoopRoundTripTime,
+ "rtLoop.RoundTripTime");
return svi;
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/ForceTorqueSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/ForceTorqueSubUnit.cpp
index 843f1799d3a11a914e6095416c75c913a976a339..6ee5adf80f1f2e409fe19bb5d31027c096fcc3a5 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/ForceTorqueSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/ForceTorqueSubUnit.cpp
@@ -25,7 +25,9 @@
#include <RobotAPI/libraries/core/FramedPose.h>
-void armarx::ForceTorqueSubUnit::update(const armarx::SensorAndControl& sc, const JointAndNJointControllers&)
+void
+armarx::ForceTorqueSubUnit::update(const armarx::SensorAndControl& sc,
+ const JointAndNJointControllers&)
{
if (!getProxy())
{
@@ -47,22 +49,26 @@ void armarx::ForceTorqueSubUnit::update(const armarx::SensorAndControl& sc, cons
listenerPrx->reportSensorValues(
dev.deviceName,
new armarx::FramedDirection(sensorVal.force, dev.frame, agentName),
- new armarx::FramedDirection(sensorVal.torque, dev.frame, agentName)
- );
+ new armarx::FramedDirection(sensorVal.torque, dev.frame, agentName));
}
}
-void armarx::ForceTorqueSubUnit::setOffset(const armarx::FramedDirectionBasePtr&, const armarx::FramedDirectionBasePtr&, const Ice::Current&)
+void
+armarx::ForceTorqueSubUnit::setOffset(const armarx::FramedDirectionBasePtr&,
+ const armarx::FramedDirectionBasePtr&,
+ const Ice::Current&)
{
ARMARX_WARNING << "NYI";
}
-void armarx::ForceTorqueSubUnit::setToNull(const Ice::Current&)
+void
+armarx::ForceTorqueSubUnit::setToNull(const Ice::Current&)
{
ARMARX_WARNING << "NYI";
}
-void armarx::ForceTorqueSubUnit::onInitForceTorqueUnit()
+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
index eb11819df743b3c30176ca046afdd8b500b28b49..6c5b294d31d9c3c7f595d1509031c5dfa4212cdd 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/ForceTorqueSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/ForceTorqueSubUnit.h
@@ -22,30 +22,38 @@
#pragma once
-#include "RobotUnitSubUnit.h"
-
#include <RobotAPI/components/units/ForceTorqueUnit.h>
#include "../SensorValues/SensorValueForceTorque.h"
+#include "RobotUnitSubUnit.h"
namespace armarx
{
TYPEDEF_PTRS_HANDLE(ForceTorqueSubUnit);
- class ForceTorqueSubUnit:
- virtual public RobotUnitSubUnit,
- virtual public ForceTorqueUnit
+
+ class ForceTorqueSubUnit : virtual public RobotUnitSubUnit, virtual public ForceTorqueUnit
{
public:
void update(const SensorAndControl& sc, const JointAndNJointControllers& c) override;
// ForceTorqueUnitInterface interface
- void setOffset(const FramedDirectionBasePtr&, const FramedDirectionBasePtr&, const Ice::Current& = Ice::emptyCurrent) override;
+ void setOffset(const FramedDirectionBasePtr&,
+ const FramedDirectionBasePtr&,
+ const Ice::Current& = Ice::emptyCurrent) override;
void setToNull(const Ice::Current& = Ice::emptyCurrent) override;
// ForceTorqueUnit interface
- void onInitForceTorqueUnit() override;
- void onStartForceTorqueUnit() override {}
- void onExitForceTorqueUnit() override {}
+ void onInitForceTorqueUnit() override;
+
+ void
+ onStartForceTorqueUnit() override
+ {
+ }
+
+ void
+ onExitForceTorqueUnit() override
+ {
+ }
struct DeviceData
{
@@ -55,7 +63,8 @@ namespace armarx
};
std::vector<DeviceData> devs;
+
private:
std::string agentName;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.cpp
index 90f43ef47f332da6e210109d55a26064b96c3662..462f0c9206698f0b503a0bf36d27e729b6c825e8 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.cpp
@@ -20,15 +20,18 @@
* GNU General Public License
*/
#include "InertialMeasurementSubUnit.h"
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-void armarx::InertialMeasurementSubUnit::onStartIMU()
+void
+armarx::InertialMeasurementSubUnit::onStartIMU()
{
ARMARX_IMPORTANT << "Devices with IMUs: " << devs;
}
-
-void armarx::InertialMeasurementSubUnit::update(const armarx::SensorAndControl& sc, const JointAndNJointControllers&)
+void
+armarx::InertialMeasurementSubUnit::update(const armarx::SensorAndControl& sc,
+ const JointAndNJointControllers&)
{
if (!getProxy())
{
@@ -50,12 +53,18 @@ void armarx::InertialMeasurementSubUnit::update(const armarx::SensorAndControl&
ARMARX_CHECK_EXPRESSION(sv->isA<SensorValueIMU>());
const SensorValueIMU* s = sv->asA<SensorValueIMU>();
IMUData data;
- data.acceleration = Ice::FloatSeq(s->linearAcceleration.data(), s->linearAcceleration.data() + s->linearAcceleration.rows() * s->linearAcceleration.cols());
- data.gyroscopeRotation = Ice::FloatSeq(s->angularVelocity.data(), s->angularVelocity.data() + s->angularVelocity.rows() * s->angularVelocity.cols());;
- data.orientationQuaternion = {s->orientation.w(), s->orientation.x(), s->orientation.y(), s->orientation.z()};
+ data.acceleration =
+ Ice::FloatSeq(s->linearAcceleration.data(),
+ s->linearAcceleration.data() +
+ s->linearAcceleration.rows() * s->linearAcceleration.cols());
+ data.gyroscopeRotation = Ice::FloatSeq(
+ s->angularVelocity.data(),
+ s->angularVelocity.data() + s->angularVelocity.rows() * s->angularVelocity.cols());
+ ;
+ data.orientationQuaternion = {
+ s->orientation.w(), s->orientation.x(), s->orientation.y(), s->orientation.z()};
auto frame = dev;
batchPrx->reportSensorValues(dev, frame, data, t);
}
batchPrx->ice_flushBatchRequests();
}
-
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.h
index d5a961e663f69caf70b9e7e4b9de194f49c0732b..4282cb834244e20c918c76b49422a4b81abc6c8a 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.h
@@ -22,18 +22,18 @@
#pragma once
-#include "RobotUnitSubUnit.h"
-
#include <ArmarXCore/observers/variant/TimestampVariant.h>
#include <RobotAPI/components/units/InertialMeasurementUnit.h>
#include "../SensorValues/SensorValueIMU.h"
+#include "RobotUnitSubUnit.h"
namespace armarx
{
TYPEDEF_PTRS_HANDLE(InertialMeasurementSubUnit);
- class InertialMeasurementSubUnit:
+
+ class InertialMeasurementSubUnit :
virtual public RobotUnitSubUnit,
virtual public InertialMeasurementUnit
{
@@ -42,10 +42,19 @@ namespace armarx
// InertialMeasurementUnit interface
protected:
- void onInitIMU() override {}
+ void
+ onInitIMU() override
+ {
+ }
+
void onStartIMU() override;
- void onExitIMU() override {}
+
+ void
+ onExitIMU() override
+ {
+ }
+
public:
std::map<std::string, std::size_t> devs;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
index 6c260f7936c42fe7cbeefefa01fcb818054bc03f..1b7f92ef951ef1b1223489b3e80fb09a84efa981 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
@@ -21,21 +21,20 @@
*/
#include "KinematicSubUnit.h"
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
-#include <RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <VirtualRobot/RobotNodeSet.h>
#include <ArmarXCore/core/util/algorithm.h>
-#include <VirtualRobot/RobotNodeSet.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include <RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.h>
-armarx::KinematicSubUnit::KinematicSubUnit() :
- reportSkipper(20.0f)
+armarx::KinematicSubUnit::KinematicSubUnit() : reportSkipper(20.0f)
{
-
}
-void armarx::KinematicSubUnit::setupData(
+void
+armarx::KinematicSubUnit::setupData(
std::string relRobFile,
VirtualRobot::RobotPtr rob,
std::map<std::string, ActuatorData>&& newDevs,
@@ -43,7 +42,7 @@ void armarx::KinematicSubUnit::setupData(
std::set<std::string> controlDeviceHardwareControlModeGrpsMerged,
RobotUnit* newRobotUnit)
{
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
ARMARX_CHECK_EXPRESSION(getState() == eManagedIceObjectCreated);
ARMARX_CHECK_EXPRESSION(!robot);
@@ -68,14 +67,17 @@ void armarx::KinematicSubUnit::setupData(
auto nodes = robot->getRobotNodes();
for (auto& node : nodes)
{
- if ((node->isRotationalJoint() || node->isTranslationalJoint()) && !devs.count(node->getName()))
+ if ((node->isRotationalJoint() || node->isTranslationalJoint()) &&
+ !devs.count(node->getName()))
{
sensorLessJoints.push_back(node);
}
}
}
-void armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc, const armarx::JointAndNJointControllers& c)
+void
+armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc,
+ const armarx::JointAndNJointControllers& c)
{
if (!getProxy())
{
@@ -88,7 +90,7 @@ void armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc, const
ARMARX_IMPORTANT << deactivateSpam(1) << "listener is not set";
return;
}
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
bool ctrlModesAValueChanged = false;
bool angAValueChanged = false;
bool velAValueChanged = false;
@@ -99,7 +101,8 @@ void armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc, const
bool statusesAvalueChanged = false;
long timestamp = sc.sensorValuesTimestamp.toMicroSeconds();
- std::set<NJointControllerBase*> nJointCtrls {c.nJointControllers.begin(), c.nJointControllers.end()};
+ std::set<NJointControllerBase*> nJointCtrls{c.nJointControllers.begin(),
+ c.nJointControllers.end()};
std::vector<std::string> actuatorsWithoutSensor;
actuatorsWithoutSensor.reserve(devs.size());
for (const auto& name2actuatorData : devs)
@@ -123,19 +126,41 @@ void armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc, const
ARMARX_CHECK_EXPRESSION(eUnknown == c);
c = eTorqueControl;
}
- ctrlModesAValueChanged = ctrlModesAValueChanged || !ctrlModes.count(name) || (ctrlModes.at(name) != c);
+ 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<float, SensorValue1DoFActuatorPosition, &SensorValue1DoFActuatorPosition::position >(ang, name, s, angAValueChanged);
- UpdateNameValueMap<float, SensorValue1DoFActuatorVelocity, &SensorValue1DoFActuatorVelocity::velocity >(vel, name, s, velAValueChanged);
- UpdateNameValueMap<float, SensorValue1DoFActuatorAcceleration, &SensorValue1DoFActuatorAcceleration::acceleration >(acc, name, s, accAValueChanged);
- UpdateNameValueMap<float, SensorValue1DoFActuatorTorque, &SensorValue1DoFActuatorTorque::torque >(tor, name, s, torAValueChanged);
- UpdateNameValueMap<float, SensorValue1DoFActuatorCurrent, &SensorValue1DoFActuatorCurrent::motorCurrent >(motorCurrents, name, s, motorCurrentAValueChanged);
- UpdateNameValueMap<float, SensorValue1DoFActuatorMotorTemperature, &SensorValue1DoFActuatorMotorTemperature::motorTemperature>(motorTemperatures, name, s, motorTemperaturesAValueChanged);
- UpdateNameValueMap<JointStatus, SensorValue1DoFActuatorStatus, &SensorValue1DoFActuatorStatus::status >(statuses, name, s, statusesAvalueChanged);
+ UpdateNameValueMap<float,
+ SensorValue1DoFActuatorPosition,
+ &SensorValue1DoFActuatorPosition::position>(
+ ang, name, s, angAValueChanged);
+ UpdateNameValueMap<float,
+ SensorValue1DoFActuatorVelocity,
+ &SensorValue1DoFActuatorVelocity::velocity>(
+ vel, name, s, velAValueChanged);
+ UpdateNameValueMap<float,
+ SensorValue1DoFActuatorAcceleration,
+ &SensorValue1DoFActuatorAcceleration::acceleration>(
+ acc, name, s, accAValueChanged);
+ UpdateNameValueMap<float,
+ SensorValue1DoFActuatorTorque,
+ &SensorValue1DoFActuatorTorque::torque>(
+ tor, name, s, torAValueChanged);
+ UpdateNameValueMap<float,
+ SensorValue1DoFActuatorCurrent,
+ &SensorValue1DoFActuatorCurrent::motorCurrent>(
+ motorCurrents, name, s, motorCurrentAValueChanged);
+ UpdateNameValueMap<float,
+ SensorValue1DoFActuatorMotorTemperature,
+ &SensorValue1DoFActuatorMotorTemperature::motorTemperature>(
+ motorTemperatures, name, s, motorTemperaturesAValueChanged);
+ UpdateNameValueMap<JointStatus,
+ SensorValue1DoFActuatorStatus,
+ &SensorValue1DoFActuatorStatus::status>(
+ statuses, name, s, statusesAvalueChanged);
}
else
{
@@ -144,7 +169,8 @@ void armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc, const
}
if (!actuatorsWithoutSensor.empty())
{
- ARMARX_WARNING << deactivateSpam(5) << "these actuators have no sensor!:\n" << actuatorsWithoutSensor;
+ ARMARX_WARNING << deactivateSpam(5) << "these actuators have no sensor!:\n"
+ << actuatorsWithoutSensor;
}
// update the joint values of linked joints
@@ -160,14 +186,17 @@ void armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc, const
}
- ARMARX_DEBUG << deactivateSpam(30) << "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";
+ ARMARX_DEBUG << deactivateSpam(30) << "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->reportJointAngles(ang, timestamp, angAValueChanged);
prx->reportJointVelocities(vel, timestamp, velAValueChanged);
@@ -181,7 +210,8 @@ void armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc, const
}
if (!motorTemperatures.empty())
{
- prx->reportJointMotorTemperatures(motorTemperatures, timestamp, motorTemperaturesAValueChanged);
+ prx->reportJointMotorTemperatures(
+ motorTemperatures, timestamp, motorTemperaturesAValueChanged);
}
if (!statuses.empty())
{
@@ -192,19 +222,22 @@ void armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc, const
prx->ice_flushBatchRequests();
}
-void armarx::KinematicSubUnit::requestJoints(const Ice::StringSeq&, const Ice::Current&)
+void
+armarx::KinematicSubUnit::requestJoints(const Ice::StringSeq&, const Ice::Current&)
{
ARMARX_WARNING << "NYI";
}
-void armarx::KinematicSubUnit::releaseJoints(const Ice::StringSeq&, const Ice::Current&)
+void
+armarx::KinematicSubUnit::releaseJoints(const Ice::StringSeq&, const Ice::Current&)
{
ARMARX_WARNING << "NYI";
}
-void armarx::KinematicSubUnit::setJointAngles(const armarx::NameValueMap& angles, const Ice::Current&)
+void
+armarx::KinematicSubUnit::setJointAngles(const armarx::NameValueMap& angles, const Ice::Current&)
{
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
for (const auto& n2v : angles)
{
if (devs.count(n2v.first))
@@ -217,9 +250,11 @@ void armarx::KinematicSubUnit::setJointAngles(const armarx::NameValueMap& angles
}
}
-void armarx::KinematicSubUnit::setJointVelocities(const armarx::NameValueMap& velocities, const Ice::Current&)
+void
+armarx::KinematicSubUnit::setJointVelocities(const armarx::NameValueMap& velocities,
+ const Ice::Current&)
{
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
for (const auto& n2v : velocities)
{
if (devs.count(n2v.first))
@@ -232,9 +267,10 @@ void armarx::KinematicSubUnit::setJointVelocities(const armarx::NameValueMap& ve
}
}
-void armarx::KinematicSubUnit::setJointTorques(const armarx::NameValueMap& torques, const Ice::Current&)
+void
+armarx::KinematicSubUnit::setJointTorques(const armarx::NameValueMap& torques, const Ice::Current&)
{
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
for (const auto& n2v : torques)
{
if (devs.count(n2v.first))
@@ -247,26 +283,33 @@ void armarx::KinematicSubUnit::setJointTorques(const armarx::NameValueMap& torqu
}
}
-void armarx::KinematicSubUnit::switchControlMode(const armarx::NameControlModeMap& ncm, const Ice::Current&)
+void
+armarx::KinematicSubUnit::switchControlMode(const armarx::NameControlModeMap& ncm,
+ const Ice::Current&)
{
std::map<std::string, NJointControllerBasePtr> toActivate;
{
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
for (const auto& n2c : ncm)
{
const std::string& name = n2c.first;
ControlMode mode = n2c.second;
if (!devs.count(name))
{
- ARMARX_WARNING << "requested mode '" << KinematicUnitHelper::ControlModeToString(mode) << "' for nonexistent device '" << name
+ ARMARX_WARNING << "requested mode '"
+ << KinematicUnitHelper::ControlModeToString(mode)
+ << "' for nonexistent device '" << name
<< "'. (ignoring this device)";
continue;
}
- NJointKinematicUnitPassThroughControllerPtr ctrl = NJointKinematicUnitPassThroughControllerPtr::dynamicCast(devs.at(name).getController(mode));
+ NJointKinematicUnitPassThroughControllerPtr ctrl =
+ NJointKinematicUnitPassThroughControllerPtr::dynamicCast(
+ devs.at(name).getController(mode));
if (!ctrl)
{
- ARMARX_WARNING << "requested unsupported mode '" << KinematicUnitHelper::ControlModeToString(mode) << "' for device '" << name
- << "'. (ignoring this device)";
+ ARMARX_WARNING << "requested unsupported mode '"
+ << KinematicUnitHelper::ControlModeToString(mode) << "' for device '"
+ << name << "'. (ignoring this device)";
continue;
}
if (ctrl->isControllerActive())
@@ -283,7 +326,9 @@ void armarx::KinematicSubUnit::switchControlMode(const armarx::NameControlModeMa
out << " '" << elem.first << "' -> '" << elem.second->getInstanceName() << "'\n";
}
};
- ARMARX_DEBUG << "switching control modes requests these NJointControllers (without consideration of ControlDeviceHardwareControlModeGroups):\n" << printToActivate;
+ ARMARX_DEBUG << "switching control modes requests these NJointControllers (without "
+ "consideration of ControlDeviceHardwareControlModeGroups):\n"
+ << printToActivate;
for (const auto& n2NJointCtrl : toActivate)
{
const auto& name = n2NJointCtrl.first;
@@ -304,7 +349,7 @@ void armarx::KinematicSubUnit::switchControlMode(const armarx::NameControlModeMa
}
}
ARMARX_CHECK_EXPRESSION(group);
- const auto jointCtrl = nJointCtrl->getControlDevicesUsedJointController().at(name);
+ const auto jointCtrl = nJointCtrl->getControlDevicesUsedJointController().at(name);
const auto hwMode = jointCtrl->getHardwareControlMode();
//check against all other elems of the group
for (const auto& other : *group)
@@ -320,36 +365,53 @@ void armarx::KinematicSubUnit::switchControlMode(const armarx::NameControlModeMa
//this device may have a controller switch
if (toActivate.count(other))
{
- const auto otherJointCtrl = toActivate.at(other)->getControlDevicesUsedJointController().at(other);
+ const auto otherJointCtrl =
+ toActivate.at(other)->getControlDevicesUsedJointController().at(other);
const auto otherHwMode = otherJointCtrl->getHardwareControlMode();
if (otherHwMode != hwMode)
{
std::stringstream strstr;
- strstr << "The hardware control mode for two control devices with requested control mode changein the same group does not match!\n"
- << "Device 1: '" << name << "' mode " << "'" << jointCtrl->getControlMode() << "' hw mode '" << hwMode << "'\n"
- << "Device 2: '" << other << "' mode " << "'" << otherJointCtrl->getControlMode() << "' hw mode '" << otherHwMode << "'\n";
+ strstr << "The hardware control mode for two control devices with "
+ "requested control mode changein the same group does not match!\n"
+ << "Device 1: '" << name << "' mode "
+ << "'" << jointCtrl->getControlMode() << "' hw mode '" << hwMode
+ << "'\n"
+ << "Device 2: '" << other << "' mode "
+ << "'" << otherJointCtrl->getControlMode() << "' hw mode '"
+ << otherHwMode << "'\n";
ARMARX_ERROR << strstr.str();
- throw InvalidArgumentException {strstr.str()};
+ throw InvalidArgumentException{strstr.str()};
}
}
else
{
//get the current active
const auto otherNJointCtrl = devs.at(other).getActiveController();
- const auto otherJointCtrl = otherNJointCtrl ? otherNJointCtrl->getControlDevicesUsedJointController().at(other) : nullptr;
- const auto otherHwMode = otherJointCtrl ? otherJointCtrl->getHardwareControlMode() : "";
+ const auto otherJointCtrl =
+ otherNJointCtrl
+ ? otherNJointCtrl->getControlDevicesUsedJointController().at(other)
+ : nullptr;
+ const auto otherHwMode =
+ otherJointCtrl ? otherJointCtrl->getHardwareControlMode() : "";
if (hwMode != otherHwMode)
{
toActivate[other] = std::move(devs.at(other).getController(ncm.at(name)));
ARMARX_CHECK_EXPRESSION(toActivate.at(other));
- ARMARX_CHECK_EXPRESSION(toActivate.at(other)->getControlDevicesUsedJointController().at(other)->getHardwareControlMode() == hwMode);
- ARMARX_VERBOSE << "activating '" << nJointCtrl->getInstanceName() << "' caused activation of '" << toActivate.at(name)->getInstanceName() << "'";
+ ARMARX_CHECK_EXPRESSION(toActivate.at(other)
+ ->getControlDevicesUsedJointController()
+ .at(other)
+ ->getHardwareControlMode() == hwMode);
+ ARMARX_VERBOSE << "activating '" << nJointCtrl->getInstanceName()
+ << "' caused activation of '"
+ << toActivate.at(name)->getInstanceName() << "'";
}
}
}
ARMARX_DEBUG << "checking group of '" << name << "'...done!";
}
- ARMARX_DEBUG << "switching control modes requests these NJointControllers (with consideration of ControlDeviceHardwareControlModeGroups):\n" << printToActivate;
+ ARMARX_DEBUG << "switching control modes requests these NJointControllers (with "
+ "consideration of ControlDeviceHardwareControlModeGroups):\n"
+ << printToActivate;
//now check if groups are satisfied
ARMARX_CHECK_EXPRESSION(robotUnit);
}
@@ -359,61 +421,65 @@ void armarx::KinematicSubUnit::switchControlMode(const armarx::NameControlModeMa
}
}
-void armarx::KinematicSubUnit::setJointAccelerations(const armarx::NameValueMap&, const Ice::Current&)
+void
+armarx::KinematicSubUnit::setJointAccelerations(const armarx::NameValueMap&, const Ice::Current&)
{
ARMARX_WARNING << "NYI";
}
-void armarx::KinematicSubUnit::setJointDecelerations(const armarx::NameValueMap&, const Ice::Current&)
+void
+armarx::KinematicSubUnit::setJointDecelerations(const armarx::NameValueMap&, const Ice::Current&)
{
ARMARX_WARNING << "NYI";
}
-armarx::NameControlModeMap armarx::KinematicSubUnit::getControlModes(const Ice::Current&)
+armarx::NameControlModeMap
+armarx::KinematicSubUnit::getControlModes(const Ice::Current&)
{
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
return ctrlModes;
}
-armarx::NameValueMap armarx::KinematicSubUnit::getJointAngles(const Ice::Current& c) const
+armarx::NameValueMap
+armarx::KinematicSubUnit::getJointAngles(const Ice::Current& c) const
{
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
return ang;
}
-armarx::NameValueMap armarx::KinematicSubUnit::getJointVelocities(const Ice::Current& c) const
+armarx::NameValueMap
+armarx::KinematicSubUnit::getJointVelocities(const Ice::Current& c) const
{
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
return vel;
}
-Ice::StringSeq armarx::KinematicSubUnit::getJoints(const Ice::Current& c) const
+Ice::StringSeq
+armarx::KinematicSubUnit::getJoints(const Ice::Current& c) const
{
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
return getMapKeys(ang);
}
-armarx::DebugInfo armarx::KinematicSubUnit::getDebugInfo(const Ice::Current& c) const
+armarx::DebugInfo
+armarx::KinematicSubUnit::getDebugInfo(const Ice::Current& c) const
{
- std::lock_guard<std::mutex> guard {dataMutex};
-
- armarx::DebugInfo debugInfo
- {
- .jointModes = ctrlModes,
- .jointAngles = ang,
- .jointVelocities = vel,
- .jointAccelerations = acc,
- .jointTorques = tor,
- .jointCurrents = motorCurrents,
- .jointMotorTemperatures = motorTemperatures,
- .jointStatus = statuses
- };
+ std::lock_guard<std::mutex> guard{dataMutex};
+
+ armarx::DebugInfo debugInfo{.jointModes = ctrlModes,
+ .jointAngles = ang,
+ .jointVelocities = vel,
+ .jointAccelerations = acc,
+ .jointTorques = tor,
+ .jointCurrents = motorCurrents,
+ .jointMotorTemperatures = motorTemperatures,
+ .jointStatus = statuses};
return debugInfo;
}
-
-armarx::NJointControllerPtr armarx::KinematicSubUnit::ActuatorData::getController(armarx::ControlMode c) const
+armarx::NJointControllerPtr
+armarx::KinematicSubUnit::ActuatorData::getController(armarx::ControlMode c) const
{
switch (c)
{
@@ -430,7 +496,8 @@ armarx::NJointControllerPtr armarx::KinematicSubUnit::ActuatorData::getControlle
}
}
-armarx::NJointControllerPtr armarx::KinematicSubUnit::ActuatorData::getActiveController() const
+armarx::NJointControllerPtr
+armarx::KinematicSubUnit::ActuatorData::getActiveController() const
{
if (ctrlPos->isControllerActive())
{
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
index 92a6caf3d845ebd2291ecf0d0ce96db597367fac..8cecc5f7e10ed0abdd29cc7e5dfb55f1260d0daf 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
@@ -25,24 +25,22 @@
#include <mutex>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/util/IceReportSkipper.h>
#include <RobotAPI/components/units/KinematicUnit.h>
-#include <ArmarXCore/core/util/IceReportSkipper.h>
-
-#include "RobotUnitSubUnit.h"
-#include "../util.h"
-#include "../SensorValues/SensorValue1DoFActuator.h"
#include "../NJointControllers/NJointKinematicUnitPassThroughController.h"
+#include "../SensorValues/SensorValue1DoFActuator.h"
+#include "../util.h"
+#include "RobotUnitSubUnit.h"
namespace armarx
{
TYPEDEF_PTRS_HANDLE(RobotUnit);
TYPEDEF_PTRS_HANDLE(KinematicSubUnit);
- class KinematicSubUnit:
- virtual public RobotUnitSubUnit,
- virtual public KinematicUnit
+
+ class KinematicSubUnit : virtual public RobotUnitSubUnit, virtual public KinematicUnit
{
public:
struct ActuatorData
@@ -56,16 +54,15 @@ namespace armarx
NJointControllerPtr getController(ControlMode c) const;
NJointControllerPtr getActiveController() const;
};
+
KinematicSubUnit();
- void setupData(
- std::string relRobFile,
- VirtualRobot::RobotPtr rob,
- std::map<std::string, ActuatorData>&& newDevs,
- std::vector<std::set<std::string> > controlDeviceHardwareControlModeGrps,
- std::set<std::string> controlDeviceHardwareControlModeGrpsMerged,
- RobotUnit* newRobotUnit
- );
+ void setupData(std::string relRobFile,
+ VirtualRobot::RobotPtr rob,
+ std::map<std::string, ActuatorData>&& newDevs,
+ std::vector<std::set<std::string>> controlDeviceHardwareControlModeGrps,
+ std::set<std::string> controlDeviceHardwareControlModeGrpsMerged,
+ RobotUnit* newRobotUnit);
void update(const SensorAndControl& sc, const JointAndNJointControllers& c) override;
// KinematicUnitInterface interface
@@ -80,17 +77,27 @@ namespace armarx
void setJointAccelerations(const NameValueMap&, const Ice::Current&) override;
void setJointDecelerations(const NameValueMap&, const Ice::Current&) override;
- NameControlModeMap getControlModes(const Ice::Current&) override;
- NameValueMap getJointAngles(const Ice::Current&) const override;
- NameValueMap getJointVelocities(const Ice::Current&) const override;
- Ice::StringSeq getJoints(const Ice::Current& c) const override;
+ NameControlModeMap getControlModes(const Ice::Current&) override;
+ NameValueMap getJointAngles(const Ice::Current&) const override;
+ NameValueMap getJointVelocities(const Ice::Current&) const override;
+ Ice::StringSeq getJoints(const Ice::Current& c) const override;
DebugInfo getDebugInfo(const Ice::Current& c = Ice::emptyCurrent) const override;
+ void
+ onInitKinematicUnit() override
+ {
+ }
- void onInitKinematicUnit() override {}
- void onStartKinematicUnit() override {}
- void onExitKinematicUnit() override {}
+ void
+ onStartKinematicUnit() override
+ {
+ }
+
+ void
+ onExitKinematicUnit() override
+ {
+ }
private:
std::map<std::string, ActuatorData> devs;
@@ -110,8 +117,12 @@ namespace armarx
IceReportSkipper reportSkipper;
std::vector<VirtualRobot::RobotNodePtr> sensorLessJoints;
- template<class ValueT, class SensorValueT, ValueT SensorValueT::* Member>
- static void UpdateNameValueMap(std::map<std::string, ValueT>& nvm, const std::string& name, const SensorValueBase* sv, bool& changeState)
+ template <class ValueT, class SensorValueT, ValueT SensorValueT::*Member>
+ static void
+ UpdateNameValueMap(std::map<std::string, ValueT>& nvm,
+ const std::string& name,
+ const SensorValueBase* sv,
+ bool& changeState)
{
const SensorValueT* s = sv->asA<SensorValueT>();
if (!s)
@@ -126,4 +137,4 @@ namespace armarx
nvm[name] = v;
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.cpp
index 12c3faf13ffb311a36994002f8f05ef7122dc206..debb6a1a7d61f2cc348dd8214dea4f5e512bdf1e 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.cpp
@@ -31,13 +31,13 @@
#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-
namespace armarx
{
LocalizationSubUnit::~LocalizationSubUnit() = default;
-
- void LocalizationSubUnit::update(const armarx::SensorAndControl& sc, const JointAndNJointControllers&)
+ void
+ LocalizationSubUnit::update(const armarx::SensorAndControl& sc,
+ const JointAndNJointControllers&)
{
if (!getProxy())
{
@@ -47,10 +47,12 @@ namespace armarx
}
}
-
- void LocalizationSubUnit::reportGlobalRobotPoseCorrection(const TransformStamped& global_T_odom, const Ice::Current&)
+ void
+ LocalizationSubUnit::reportGlobalRobotPoseCorrection(const TransformStamped& global_T_odom,
+ const Ice::Current&)
{
ARMARX_CHECK_EXPRESSION(globalPositionCorrectionSensorDevice);
- globalPositionCorrectionSensorDevice->updateGlobalPositionCorrection(global_T_odom.transform);
+ globalPositionCorrectionSensorDevice->updateGlobalPositionCorrection(
+ global_T_odom.transform);
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h
index f628ecc3164f02134d1a4c8c2e9b5915295e5762..c40aa7e4460792dae1ae0e8f5519edb80766a31f 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h
@@ -29,9 +29,9 @@
#include <VirtualRobot/MathTools.h>
#include <RobotAPI/components/units/SensorActorUnit.h>
-#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/units/LocalizationUnitInterface.h>
+#include <RobotAPI/libraries/core/Pose.h>
#include "RobotUnitSubUnit.h"
@@ -44,21 +44,29 @@ namespace armarx
{
public:
// inherited from Component
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "LocalizationUnit";
}
- void onInitComponent() override {}
- void onConnectComponent() override {}
- };
+ void
+ onInitComponent() override
+ {
+ }
+ void
+ onConnectComponent() override
+ {
+ }
+ };
class GlobalRobotPoseCorrectionSensorDevice;
TYPEDEF_PTRS_HANDLE(LocalizationSubUnit);
- class LocalizationSubUnit:
+
+ class LocalizationSubUnit :
virtual public RobotUnitSubUnit,
virtual public LocalizationUnit,
virtual public LocalizationSubUnitInterface
@@ -69,7 +77,8 @@ namespace armarx
void update(const SensorAndControl& sc, const JointAndNJointControllers& c) override;
- void reportGlobalRobotPoseCorrection(const TransformStamped& global_T_odom, const Ice::Current& = Ice::emptyCurrent) override;
+ void reportGlobalRobotPoseCorrection(const TransformStamped& global_T_odom,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* This device partially holds the information about the robot's global pose.
@@ -80,10 +89,7 @@ namespace armarx
GlobalRobotPoseCorrectionSensorDevice* globalPositionCorrectionSensorDevice;
private:
-
std::string agentName;
std::string robotRootFrame;
-
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
index 5ae63c2ac2f2c7ba30ac903feeddeb9a4fed6814..c7e7348c57092485ddc70a1e5a18487b091cd358 100755
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
@@ -26,16 +26,17 @@
#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
+#include <RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h>
#include <RobotAPI/interface/core/GeometryBase.h>
#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h>
-
namespace armarx
{
- void armarx::PlatformSubUnit::update(const armarx::SensorAndControl& sc, const JointAndNJointControllers&)
+ void
+ armarx::PlatformSubUnit::update(const armarx::SensorAndControl& sc,
+ const JointAndNJointControllers&)
{
if (!getProxy())
{
@@ -50,9 +51,10 @@ namespace armarx
}
const SensorValueBase* sensorValue = sc.sensors.at(platformSensorIndex).get();
ARMARX_CHECK_EXPRESSION(sensorValue);
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
- const auto timestamp = sc.sensorValuesTimestamp.toMicroSeconds();;
+ const auto timestamp = sc.sensorValuesTimestamp.toMicroSeconds();
+ ;
// odom velocity is in local robot frame
FrameHeader odomVelocityHeader;
@@ -78,10 +80,13 @@ namespace armarx
//pos
{
- ARMARX_CHECK_EXPRESSION(sensorValue->isA<SensorValueHolonomicPlatformRelativePosition>());
- const SensorValueHolonomicPlatformRelativePosition* s = sensorValue->asA<SensorValueHolonomicPlatformRelativePosition>();
+ ARMARX_CHECK_EXPRESSION(
+ sensorValue->isA<SensorValueHolonomicPlatformRelativePosition>());
+ const SensorValueHolonomicPlatformRelativePosition* s =
+ sensorValue->asA<SensorValueHolonomicPlatformRelativePosition>();
- const auto odom_T_robot = VirtualRobot::MathTools::posrpy2eigen4f(s->relativePositionX, s->relativePositionY, 0, 0, 0, s->relativePositionRotation);
+ const auto odom_T_robot = VirtualRobot::MathTools::posrpy2eigen4f(
+ s->relativePositionX, s->relativePositionY, 0, 0, 0, s->relativePositionRotation);
// @@@ CHECK BELOW:
// if (s->isA<SensorValueHolonomicPlatformAbsolutePosition>())
@@ -108,22 +113,20 @@ namespace armarx
// const auto global_T_robot = global_T_odom * odom_T_robot;
- const TransformStamped odomPose
- {
- .header = odomPoseHeader,
- .transform = odom_T_robot
- };
+ const TransformStamped odomPose{.header = odomPoseHeader, .transform = odom_T_robot};
odometryPrx->reportOdometryPose(odomPose);
// legacy interface
const auto odomLegacyPose = toPlatformPose(odomPose);
- listenerPrx->reportPlatformOdometryPose(odomLegacyPose.x, odomLegacyPose.y, odomLegacyPose.rotationAroundZ);
+ listenerPrx->reportPlatformOdometryPose(
+ odomLegacyPose.x, odomLegacyPose.y, odomLegacyPose.rotationAroundZ);
}
-
+
//vel
{
ARMARX_CHECK_EXPRESSION(sensorValue->isA<SensorValueHolonomicPlatformVelocity>());
- const SensorValueHolonomicPlatformVelocity* s = sensorValue->asA<SensorValueHolonomicPlatformVelocity>();
+ const SensorValueHolonomicPlatformVelocity* s =
+ sensorValue->asA<SensorValueHolonomicPlatformVelocity>();
TwistStamped odomVelocity;
odomVelocity.header = odomVelocityHeader;
@@ -138,22 +141,27 @@ namespace armarx
}
}
- void armarx::PlatformSubUnit::move(Ice::Float vx, Ice::Float vy, Ice::Float vr, const Ice::Current&)
+ 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(
- std::clamp(vx, -maxVLin, maxVLin),
- std::clamp(vy, -maxVLin, maxVLin),
- std::clamp(vr, -maxVAng, maxVAng)
- );
+ std::lock_guard<std::mutex> guard{dataMutex};
+ pt->setVelocites(std::clamp(vx, -maxVLin, maxVLin),
+ std::clamp(vy, -maxVLin, maxVLin),
+ std::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&)
+ void
+ armarx::PlatformSubUnit::moveTo(Ice::Float rx,
+ Ice::Float ry,
+ Ice::Float rr,
+ Ice::Float lac,
+ Ice::Float rac,
+ const Ice::Current&)
{
globalPosCtrl->setTarget(rx, ry, rr, lac, rac);
if (!globalPosCtrl->isControllerActive())
@@ -162,7 +170,13 @@ namespace armarx
};
}
- void armarx::PlatformSubUnit::moveRelative(Ice::Float rx, Ice::Float ry, Ice::Float rr, Ice::Float lac, Ice::Float rac, const Ice::Current&)
+ void
+ armarx::PlatformSubUnit::moveRelative(Ice::Float rx,
+ Ice::Float ry,
+ Ice::Float rr,
+ Ice::Float lac,
+ Ice::Float rac,
+ const Ice::Current&)
{
relativePosCtrl->setTarget(rx, ry, rr, lac, rac);
if (!relativePosCtrl->isControllerActive())
@@ -172,12 +186,14 @@ namespace armarx
//holding the mutex here could deadlock
// std::lock_guard<std::mutex> guard {dataMutex};
ARMARX_INFO << "target orientation: " << rr;
-
}
- void armarx::PlatformSubUnit::setMaxVelocities(Ice::Float mxVLin, Ice::Float mxVAng, const Ice::Current&)
+ void
+ armarx::PlatformSubUnit::setMaxVelocities(Ice::Float mxVLin,
+ Ice::Float mxVAng,
+ const Ice::Current&)
{
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
maxVLin = std::abs(mxVLin);
maxVAng = std::abs(mxVAng);
}
@@ -195,10 +211,11 @@ namespace armarx
// return new Pose {global_T_robot};
// }
- void armarx::PlatformSubUnit::stopPlatform(const Ice::Current& c)
+ void
+ armarx::PlatformSubUnit::stopPlatform(const Ice::Current& c)
{
move(0, 0, 0);
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
index 93ea9a009c91a8c46b2c96ac0ac4917d0fb36850..90d5443bfd71a795ba244f650ae97b24988359e2 100755
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
@@ -29,21 +29,22 @@
#include <VirtualRobot/MathTools.h>
#include <RobotAPI/components/units/PlatformUnit.h>
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h>
-#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h>
#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/libraries/core/Pose.h>
-#include "RobotUnitSubUnit.h"
#include "../NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h"
#include "../SensorValues/SensorValueHolonomicPlatform.h"
+#include "RobotUnitSubUnit.h"
namespace armarx
{
class GlobalRobotPoseCorrectionSensorDevice;
TYPEDEF_PTRS_HANDLE(PlatformSubUnit);
- class PlatformSubUnit:
+
+ class PlatformSubUnit :
virtual public RobotUnitSubUnit,
virtual public PlatformUnit,
virtual public PlatformSubUnitInterface
@@ -52,22 +53,44 @@ namespace armarx
void update(const SensorAndControl& sc, const JointAndNJointControllers& c) override;
// PlatformUnitInterface interface
- void move(Ice::Float vx, Ice::Float vy, Ice::Float vr, const Ice::Current& = Ice::emptyCurrent) override;
- void moveTo(Ice::Float rx, Ice::Float ry, Ice::Float rr, Ice::Float lac, Ice::Float rac, const Ice::Current& = Ice::emptyCurrent) override;
- void moveRelative(Ice::Float rx, Ice::Float ry, Ice::Float rr, Ice::Float lac, Ice::Float rac, const Ice::Current& = Ice::emptyCurrent) override;
- void setMaxVelocities(Ice::Float mxVLin, Ice::Float mxVAng, const Ice::Current& = Ice::emptyCurrent) override;
-
+ void move(Ice::Float vx,
+ Ice::Float vy,
+ Ice::Float vr,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void moveTo(Ice::Float rx,
+ Ice::Float ry,
+ Ice::Float rr,
+ Ice::Float lac,
+ Ice::Float rac,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void moveRelative(Ice::Float rx,
+ Ice::Float ry,
+ Ice::Float rr,
+ Ice::Float lac,
+ Ice::Float rac,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setMaxVelocities(Ice::Float mxVLin,
+ Ice::Float mxVAng,
+ const Ice::Current& = Ice::emptyCurrent) override;
// PlatformUnit interface
- void onInitPlatformUnit() override
+ void
+ onInitPlatformUnit() override
{
}
- void onStartPlatformUnit() override
+
+ void
+ onStartPlatformUnit() override
{
agentName = robotStateComponent->getRobotName();
robotRootFrame = robotStateComponent->getSynchronizedRobot()->getRootNode()->getName();
}
- void onExitPlatformUnit() override {}
+
+ void
+ onExitPlatformUnit() override
+ {
+ }
+
void stopPlatform(const Ice::Current& c = Ice::emptyCurrent) override;
NJointHolonomicPlatformUnitVelocityPassThroughControllerPtr pt;
@@ -77,7 +100,6 @@ namespace armarx
std::size_t platformSensorIndex;
protected:
-
mutable std::mutex dataMutex;
Ice::Float maxVLin = std::numeric_limits<Ice::Float>::infinity();
@@ -85,10 +107,7 @@ namespace armarx
private:
-
std::string agentName;
std::string robotRootFrame;
-
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h
index 8d1d4db3501edd1e6198c8bf9d4457aef9bb657c..3d4aae806be9068fdba2016b8200c343c130f4cb 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h
@@ -25,15 +25,16 @@
#include <ArmarXCore/core/ManagedIceObject.h>
#include "../util.h"
-#include "../util/JointAndNJointControllers.h"
#include "../util/ControlThreadOutputBuffer.h"
+#include "../util/JointAndNJointControllers.h"
namespace armarx
{
TYPEDEF_PTRS_HANDLE(RobotUnitSubUnit);
- class RobotUnitSubUnit: public virtual ManagedIceObject
+
+ class RobotUnitSubUnit : public virtual ManagedIceObject
{
public:
virtual void update(const SensorAndControl& sc, const JointAndNJointControllers& c) = 0;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
index 67b7eec6c4b3aa1dec80caa3efc7108adbaef84c..1f4c16dc051771c747448b2536968fda71620748 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
@@ -22,26 +22,29 @@
#include "TCPControllerSubUnit.h"
-#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
using namespace armarx;
-void TCPControllerSubUnit::update(const SensorAndControl& sc, const JointAndNJointControllers& c)
+void
+TCPControllerSubUnit::update(const SensorAndControl& sc, const JointAndNJointControllers& c)
{
-
}
-PropertyDefinitionsPtr TCPControllerSubUnit::createPropertyDefinitions()
+PropertyDefinitionsPtr
+TCPControllerSubUnit::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new TCPControllerSubUnitPropertyDefinitions(getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new TCPControllerSubUnitPropertyDefinitions(getConfigIdentifier()));
}
-void TCPControllerSubUnit::setup(RobotUnit* rUnit, VirtualRobot::RobotPtr robot)
+void
+TCPControllerSubUnit::setup(RobotUnit* rUnit, VirtualRobot::RobotPtr robot)
{
ARMARX_CHECK_EXPRESSION(robot);
ARMARX_CHECK_EXPRESSION(!this->coordinateTransformationRobot);
@@ -52,15 +55,23 @@ void TCPControllerSubUnit::setup(RobotUnit* rUnit, VirtualRobot::RobotPtr robot)
robotUnit = rUnit;
}
-void TCPControllerSubUnit::setCycleTime(Ice::Int, const Ice::Current& c)
+void
+TCPControllerSubUnit::setCycleTime(Ice::Int, const Ice::Current& c)
{
- ARMARX_WARNING << deactivateSpam() << "Setting cycle time in RT-Controller does not have an effect";
+ ARMARX_WARNING << deactivateSpam()
+ << "Setting cycle time in RT-Controller does not have an effect";
}
-void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const std::string& tcpName, const FramedDirectionBasePtr& translationVelocity, const FramedDirectionBasePtr& orientationVelocityRPY, const Ice::Current& c)
+void
+TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName,
+ const std::string& tcpName,
+ const FramedDirectionBasePtr& translationVelocity,
+ const FramedDirectionBasePtr& orientationVelocityRPY,
+ const Ice::Current& c)
{
std::unique_lock lock(dataMutex);
- ARMARX_CHECK_EXPRESSION(coordinateTransformationRobot->hasRobotNodeSet(nodeSetName)) << "The robot does not have the node set: " + nodeSetName;
+ ARMARX_CHECK_EXPRESSION(coordinateTransformationRobot->hasRobotNodeSet(nodeSetName))
+ << "The robot does not have the node set: " + nodeSetName;
std::string tcp;
if (tcpName.empty())
{
@@ -68,7 +79,8 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
}
else
{
- ARMARX_CHECK_EXPRESSION(coordinateTransformationRobot->hasRobotNode(tcpName)) << "The robot does not have the node: " + tcpName;
+ ARMARX_CHECK_EXPRESSION(coordinateTransformationRobot->hasRobotNode(tcpName))
+ << "The robot does not have the node: " + tcpName;
tcp = tcpName;
}
@@ -85,7 +97,8 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
FramedDirectionPtr globalTransVel = FramedDirectionPtr::dynamicCast(translationVelocity);
if (globalTransVel)
{
- globalTransVel->changeFrame(coordinateTransformationRobot, coordinateTransformationRobot->getRootNode()->getName());
+ globalTransVel->changeFrame(coordinateTransformationRobot,
+ coordinateTransformationRobot->getRootNode()->getName());
xVel = globalTransVel->x;
yVel = globalTransVel->y;
zVel = globalTransVel->z;
@@ -94,11 +107,11 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
FramedDirectionPtr globalOriVel = FramedDirectionPtr::dynamicCast(orientationVelocityRPY);
if (globalOriVel)
{
- globalOriVel->changeFrame(coordinateTransformationRobot, coordinateTransformationRobot->getRootNode()->getName());
+ globalOriVel->changeFrame(coordinateTransformationRobot,
+ coordinateTransformationRobot->getRootNode()->getName());
rollVel = globalOriVel->x;
pitchVel = globalOriVel->y;
yawVel = globalOriVel->z;
-
}
CartesianSelectionMode::CartesianSelection mode = CartesianSelectionMode::eAll;
@@ -120,7 +133,8 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
noMode = true;
}
ARMARX_DEBUG << "CartesianSelection-Mode: " << (int)mode;
- auto controllerName = this->getName() + "_" + tcp + "_" + nodeSetName + "_mode_" + std::to_string((int)mode);
+ auto controllerName =
+ this->getName() + "_" + tcp + "_" + nodeSetName + "_mode_" + std::to_string((int)mode);
auto NJointControllers = robotUnit->getNJointControllerNames();
NJointCartesianVelocityControllerWithRampPtr tcpController;
bool nodeSetAlreadyControlled = false;
@@ -141,7 +155,8 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
{
continue;
}
- if (tcpController->getNodeSetName() == nodeSetName && tcpController->getInstanceName() == controllerName)
+ if (tcpController->getNodeSetName() == nodeSetName &&
+ tcpController->getInstanceName() == controllerName)
{
nodeSetAlreadyControlled = true;
break;
@@ -150,22 +165,25 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
if (!nodeSetAlreadyControlled)
{
- NJointCartesianVelocityControllerWithRampConfigPtr config = new NJointCartesianVelocityControllerWithRampConfig(nodeSetName, tcp, mode, 500, 1, 2, 0.3f, 2);
+ NJointCartesianVelocityControllerWithRampConfigPtr config =
+ new NJointCartesianVelocityControllerWithRampConfig(
+ nodeSetName, tcp, mode, 500, 1, 2, 0.3f, 2);
// NJointCartesianVelocityControllerConfigPtr config = new NJointCartesianVelocityControllerConfig(nodeSetName, tcp, mode);
- NJointCartesianVelocityControllerWithRampPtr ctrl = NJointCartesianVelocityControllerWithRampPtr::dynamicCast(
- robotUnit->createNJointController(
- "NJointCartesianVelocityControllerWithRamp", controllerName,
- config, true, true
- )
- );
+ NJointCartesianVelocityControllerWithRampPtr ctrl =
+ NJointCartesianVelocityControllerWithRampPtr::dynamicCast(
+ robotUnit->createNJointController("NJointCartesianVelocityControllerWithRamp",
+ controllerName,
+ config,
+ true,
+ true));
tcpController = ctrl;
tcpControllerNameMap[nodeSetName] = controllerName;
- tcpController->setKpJointLimitAvoidance(getProperty<float>("AvoidJointLimitsKp").getValue(), c);
+ tcpController->setKpJointLimitAvoidance(getProperty<float>("AvoidJointLimitsKp").getValue(),
+ c);
}
-
// Only activate controller if at least either translationVelocity or orientaionVelocity is set
if (!noMode)
{
@@ -184,7 +202,8 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
}
}
-bool TCPControllerSubUnit::isRequested(const Ice::Current&)
+bool
+TCPControllerSubUnit::isRequested(const Ice::Current&)
{
std::unique_lock lock(dataMutex);
for (auto& pair : tcpControllerNameMap)
@@ -198,15 +217,19 @@ bool TCPControllerSubUnit::isRequested(const Ice::Current&)
return false;
}
-void armarx::TCPControllerSubUnit::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
+void
+armarx::TCPControllerSubUnit::componentPropertiesUpdated(
+ const std::set<std::string>& changedProperties)
{
if (changedProperties.count("AvoidJointLimitsKp") && robotUnit)
{
float avoidJointLimitsKp = getProperty<float>("AvoidJointLimitsKp").getValue();
- auto activeNJointControllers = robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
+ auto activeNJointControllers =
+ robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
for (NJointControllerBasePtr controller : activeNJointControllers)
{
- auto tcpController = NJointCartesianVelocityControllerWithRampPtr::dynamicCast(controller);
+ auto tcpController =
+ NJointCartesianVelocityControllerWithRampPtr::dynamicCast(controller);
if (tcpController)
{
tcpController->setKpJointLimitAvoidance(avoidJointLimitsKp, Ice::emptyCurrent);
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
index b952748176b9940a8c5529775faaadb22ce574ab..3de0d7d81f61c7f8ba469b0aa964973d5c4d37b1 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
@@ -22,30 +22,36 @@
#pragma once
+#include <mutex>
+
+#include <ArmarXCore/core/Component.h>
+
#include <RobotAPI/interface/units/TCPControlUnit.h>
-#include "RobotUnitSubUnit.h"
#include "../NJointControllers/NJointTCPController.h"
#include "../util.h"
-
-#include <ArmarXCore/core/Component.h>
-
-#include <mutex>
+#include "RobotUnitSubUnit.h"
namespace armarx
{
TYPEDEF_PTRS_HANDLE(RobotUnit);
+
class TCPControllerSubUnitPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
- TCPControllerSubUnitPropertyDefinitions(std::string prefix):
+ TCPControllerSubUnitPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<float>("AvoidJointLimitsKp", 1.f, "Proportional gain value of P-Controller for joint limit avoidance", PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>(
+ "AvoidJointLimitsKp",
+ 1.f,
+ "Proportional gain value of P-Controller for joint limit avoidance",
+ PropertyDefinitionBase::eModifiable);
}
};
TYPEDEF_PTRS_HANDLE(TCPControllerSubUnit);
+
class TCPControllerSubUnit :
virtual public RobotUnitSubUnit,
virtual public TCPControlUnitInterface,
@@ -55,12 +61,18 @@ namespace armarx
void setup(RobotUnit* rUnit, VirtualRobot::RobotPtr robot);
// TCPControlUnitInterface interface
- void setCycleTime(Ice::Int milliseconds, const Ice::Current& c = Ice::emptyCurrent) override;
- void setTCPVelocity(const std::string& nodeSetName, const std::string& tcpName, const::armarx::FramedDirectionBasePtr& translationVelocity, const::armarx::FramedDirectionBasePtr& orientationVelocityRPY, const Ice::Current& c = Ice::emptyCurrent) override;
+ void setCycleTime(Ice::Int milliseconds,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void setTCPVelocity(const std::string& nodeSetName,
+ const std::string& tcpName,
+ const ::armarx::FramedDirectionBasePtr& translationVelocity,
+ const ::armarx::FramedDirectionBasePtr& orientationVelocityRPY,
+ const Ice::Current& c = Ice::emptyCurrent) override;
bool isRequested(const Ice::Current& = Ice::emptyCurrent) override;
// RobotUnitSubUnit interface
void update(const SensorAndControl& sc, const JointAndNJointControllers& c) override;
+
private:
mutable std::mutex dataMutex;
RobotUnit* robotUnit = nullptr;
@@ -69,48 +81,109 @@ namespace armarx
// ManagedIceObject interface
protected:
- void onInitComponent() override {}
- void onConnectComponent() override {}
- std::string getDefaultName() const override
+ void
+ onInitComponent() override
{
- return "TCPControlUnit";
}
+ void
+ onConnectComponent() override
+ {
+ }
+
+ std::string
+ getDefaultName() const override
+ {
+ return "TCPControlUnit";
+ }
PropertyDefinitionsPtr createPropertyDefinitions() override;
// UnitResourceManagementInterface interface
public:
- void request(const Ice::Current&) override
+ void
+ request(const Ice::Current&) override
{
}
- void release(const Ice::Current&) override
+
+ void
+ release(const Ice::Current&) override
{
}
// UnitExecutionManagementInterface interface
public:
- void init(const Ice::Current&) override {}
- void start(const Ice::Current&) override {}
- void stop(const Ice::Current&) override {}
- UnitExecutionState getExecutionState(const Ice::Current&) override
+ void
+ init(const Ice::Current&) override
+ {
+ }
+
+ void
+ start(const Ice::Current&) override
+ {
+ }
+
+ void
+ stop(const Ice::Current&) override
+ {
+ }
+
+ UnitExecutionState
+ getExecutionState(const Ice::Current&) override
{
return UnitExecutionState::eUndefinedUnitExecutionState;
}
// KinematicUnitListener interface
public:
- void reportControlModeChanged(const NameControlModeMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointAngles(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointVelocities(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointTorques(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointAccelerations(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointCurrents(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointMotorTemperatures(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&) override {}
+ void
+ reportControlModeChanged(const NameControlModeMap&,
+ Ice::Long,
+ bool,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointAngles(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointVelocities(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointTorques(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointAccelerations(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointCurrents(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointMotorTemperatures(const NameValueMap&,
+ Ice::Long,
+ bool,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
// PropertyUser interface
public:
void componentPropertiesUpdated(const std::set<std::string>& changedProperties) override;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
index e22d0319c205a8665bb23222f692b1832b417b41..fa79ff88f5f8e52d25289868d91a7d9194f2d5f5 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
@@ -24,7 +24,8 @@
using namespace armarx;
-void TrajectoryControllerSubUnit::onInitComponent()
+void
+TrajectoryControllerSubUnit::onInitComponent()
{
kinematicUnitName = getProperty<std::string>("KinematicUnitName").getValue();
usingProxy(kinematicUnitName);
@@ -34,23 +35,27 @@ void TrajectoryControllerSubUnit::onInitComponent()
kp = getProperty<float>("Kp").getValue();
ki = getProperty<float>("Ki").getValue();
kd = getProperty<float>("Kd").getValue();
- maxVelocity = getProperty<float>("absMaximumVelocity").getValue() / 180 * M_PI; // config expects value in rad/sec
+ maxVelocity = getProperty<float>("absMaximumVelocity").getValue() / 180 *
+ M_PI; // config expects value in rad/sec
offeringTopic("DebugDrawerUpdates");
}
-void TrajectoryControllerSubUnit::onConnectComponent()
+void
+TrajectoryControllerSubUnit::onConnectComponent()
{
kinematicUnit = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
debugDrawer = getTopic<armarx::DebugDrawerInterfacePrx>("DebugDrawerUpdates");
}
-void TrajectoryControllerSubUnit::onDisconnectComponent()
+void
+TrajectoryControllerSubUnit::onDisconnectComponent()
{
debugDrawer->clearLayer("Preview");
}
-bool TrajectoryControllerSubUnit::startTrajectoryPlayer(const Ice::Current& c)
+bool
+TrajectoryControllerSubUnit::startTrajectoryPlayer(const Ice::Current& c)
{
ARMARX_DEBUG << "Starting TrajectoryPlayer ...";
@@ -65,17 +70,28 @@ bool TrajectoryControllerSubUnit::startTrajectoryPlayer(const Ice::Current& c)
{
std::string fileName = kinematicUnit->getRobotFilename();
ARMARX_INFO << "robot file name : " << fileName;
- debugDrawer->setRobotVisu("Preview", "previewRobot", fileName, fileName.substr(0, fileName.find("/")), armarx::CollisionModel);
- debugDrawer->updateRobotColor("Preview", "previewRobot", DrawColor {0, 1, 0, 0.5});
-
- previewTask = new PeriodicTask<TrajectoryControllerSubUnit>(this, &TrajectoryControllerSubUnit::previewRun, 20, false, "TrajectoryControllerSubUnitPreviewTask", false);
+ debugDrawer->setRobotVisu("Preview",
+ "previewRobot",
+ fileName,
+ fileName.substr(0, fileName.find("/")),
+ armarx::CollisionModel);
+ debugDrawer->updateRobotColor("Preview", "previewRobot", DrawColor{0, 1, 0, 0.5});
+
+ previewTask =
+ new PeriodicTask<TrajectoryControllerSubUnit>(this,
+ &TrajectoryControllerSubUnit::previewRun,
+ 20,
+ false,
+ "TrajectoryControllerSubUnitPreviewTask",
+ false);
previewTask->start();
}
return true;
}
-bool TrajectoryControllerSubUnit::pauseTrajectoryPlayer(const Ice::Current& c)
+bool
+TrajectoryControllerSubUnit::pauseTrajectoryPlayer(const Ice::Current& c)
{
if (controllerExists())
{
@@ -93,7 +109,8 @@ bool TrajectoryControllerSubUnit::pauseTrajectoryPlayer(const Ice::Current& c)
return true;
}
-bool TrajectoryControllerSubUnit::stopTrajectoryPlayer(const Ice::Current& c)
+bool
+TrajectoryControllerSubUnit::stopTrajectoryPlayer(const Ice::Current& c)
{
ARMARX_DEBUG << "Stopping TrajectoryPlayer ...";
@@ -106,20 +123,25 @@ bool TrajectoryControllerSubUnit::stopTrajectoryPlayer(const Ice::Current& c)
}
jointTrajController->deactivateController();
- while (jointTrajController->isControllerActive()) {}
+ while (jointTrajController->isControllerActive())
+ {
+ }
jointTrajController->deleteController();
}
return true;
}
-bool TrajectoryControllerSubUnit::resetTrajectoryPlayer(bool moveToFrameZeroPose, const Ice::Current& c)
+bool
+TrajectoryControllerSubUnit::resetTrajectoryPlayer(bool moveToFrameZeroPose, const Ice::Current& c)
{
ARMARX_DEBUG << "Resetting TrajectoryPlayer ...";
if (controllerExists() && jointTrajController->isControllerActive())
{
jointTrajController->deactivateController();
- while (jointTrajController->isControllerActive()) {}
+ while (jointTrajController->isControllerActive())
+ {
+ }
}
if (!jointTraj)
@@ -133,7 +155,7 @@ bool TrajectoryControllerSubUnit::resetTrajectoryPlayer(bool moveToFrameZeroPose
jointTrajController->setTrajectory(this->jointTraj, c);
if (moveToFrameZeroPose)
{
- std::vector<Ice::DoubleSeq > states = jointTraj->getAllStates(0.0f, 1);
+ std::vector<Ice::DoubleSeq> states = jointTraj->getAllStates(0.0f, 1);
NameValueMap frameZeroPositions;
NameControlModeMap controlModes;
@@ -154,7 +176,9 @@ bool TrajectoryControllerSubUnit::resetTrajectoryPlayer(bool moveToFrameZeroPose
return true;
}
-void TrajectoryControllerSubUnit::loadJointTraj(const TrajectoryBasePtr& jointTraj, const Ice::Current& c)
+void
+TrajectoryControllerSubUnit::loadJointTraj(const TrajectoryBasePtr& jointTraj,
+ const Ice::Current& c)
{
ARMARX_DEBUG << "Loading Trajectory ...";
@@ -194,7 +218,8 @@ void TrajectoryControllerSubUnit::loadJointTraj(const TrajectoryBasePtr& jointTr
if (usedJoints.size() != this->jointTraj->dim())
{
- ARMARX_ERROR << "Not all trajectory dimensions are named !!! (would cause problems when using kinematicUnit)";
+ ARMARX_ERROR << "Not all trajectory dimensions are named !!! (would cause problems when "
+ "using kinematicUnit)";
return;
}
@@ -208,12 +233,15 @@ void TrajectoryControllerSubUnit::loadJointTraj(const TrajectoryBasePtr& jointTr
trajEndTime = endTime;
}
-void TrajectoryControllerSubUnit::loadBasePoseTraj(const TrajectoryBasePtr& basePoseTraj, const Ice::Current& c)
+void
+TrajectoryControllerSubUnit::loadBasePoseTraj(const TrajectoryBasePtr& basePoseTraj,
+ const Ice::Current& c)
{
this->basePoseTraj = TrajectoryPtr::dynamicCast(basePoseTraj);
}
-void TrajectoryControllerSubUnit::setLoopPlayback(bool loop, const Ice::Current& c)
+void
+TrajectoryControllerSubUnit::setLoopPlayback(bool loop, const Ice::Current& c)
{
std::unique_lock lock(dataMutex);
@@ -225,17 +253,20 @@ void TrajectoryControllerSubUnit::setLoopPlayback(bool loop, const Ice::Current&
jointTrajController->setLooping(loop);
}
-double TrajectoryControllerSubUnit::getEndTime(const Ice::Current& c)
+double
+TrajectoryControllerSubUnit::getEndTime(const Ice::Current& c)
{
return endTime;
}
-double TrajectoryControllerSubUnit::getTrajEndTime(const Ice::Current& c)
+double
+TrajectoryControllerSubUnit::getTrajEndTime(const Ice::Current& c)
{
return trajEndTime;
}
-double TrajectoryControllerSubUnit::getCurrentTime(const Ice::Current& c)
+double
+TrajectoryControllerSubUnit::getCurrentTime(const Ice::Current& c)
{
std::unique_lock lock(dataMutex);
@@ -247,7 +278,8 @@ double TrajectoryControllerSubUnit::getCurrentTime(const Ice::Current& c)
return jointTrajController->getCurrentTrajTime();
}
-void TrajectoryControllerSubUnit::setEndTime(Ice::Double end, const Ice::Current& c)
+void
+TrajectoryControllerSubUnit::setEndTime(Ice::Double end, const Ice::Current& c)
{
ARMARX_DEBUG << "Setting end-time ...";
@@ -262,7 +294,8 @@ void TrajectoryControllerSubUnit::setEndTime(Ice::Double end, const Ice::Current
if (jointTrajController->isControllerActive())
{
- ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting a new end-time.";
+ ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting a "
+ "new end-time.";
return;
}
@@ -286,11 +319,13 @@ void TrajectoryControllerSubUnit::setEndTime(Ice::Double end, const Ice::Current
considerConstraintsForTrajectoryOptimization = b;
}
-void TrajectoryControllerSubUnit::setIsPreview(bool isPreview, const Ice::Current& c)
+void
+TrajectoryControllerSubUnit::setIsPreview(bool isPreview, const Ice::Current& c)
{
if (controllerExists() && jointTrajController->isControllerActive())
{
- ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting whether the controller is only for preview.";
+ ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting "
+ "whether the controller is only for preview.";
return;
}
this->isPreview = isPreview;
@@ -302,7 +337,10 @@ void TrajectoryControllerSubUnit::setIsPreview(bool isPreview, const Ice::Curren
}
}
-bool TrajectoryControllerSubUnit::setJointsInUse(const std::string& jointName, bool isInUse, const Ice::Current& c)
+bool
+TrajectoryControllerSubUnit::setJointsInUse(const std::string& jointName,
+ bool isInUse,
+ const Ice::Current& c)
{
ARMARX_DEBUG << "Setting joints in use ...";
@@ -310,11 +348,13 @@ bool TrajectoryControllerSubUnit::setJointsInUse(const std::string& jointName, b
if (controllerExists())
{
- ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting joints in use.";
+ ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting "
+ "joints in use.";
}
else
{
- if (isInUse && (std::find(usedJoints.begin(), usedJoints.end(), jointName) == usedJoints.end()))
+ if (isInUse &&
+ (std::find(usedJoints.begin(), usedJoints.end(), jointName) == usedJoints.end()))
{
usedJoints.push_back(jointName);
}
@@ -338,21 +378,24 @@ bool TrajectoryControllerSubUnit::setJointsInUse(const std::string& jointName, b
return isInUse;
}
-void TrajectoryControllerSubUnit::enableRobotPoseUnit(bool isRobotPose, const Ice::Current& c)
+void
+TrajectoryControllerSubUnit::enableRobotPoseUnit(bool isRobotPose, const Ice::Current& c)
{
ARMARX_WARNING << "NYI : TrajectoryControllerSubUnit::enableRobotPoseUnit()";
}
-void TrajectoryControllerSubUnit::update(const SensorAndControl& sc, const JointAndNJointControllers& c)
+void
+TrajectoryControllerSubUnit::update(const SensorAndControl& sc, const JointAndNJointControllers& c)
{
-
}
-void TrajectoryControllerSubUnit::considerConstraints(bool consider, const Ice::Current& c)
+void
+TrajectoryControllerSubUnit::considerConstraints(bool consider, const Ice::Current& c)
{
if (controllerExists() && jointTrajController->isControllerActive())
{
- ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting whether constraints should be considered.";
+ ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting "
+ "whether constraints should be considered.";
return;
}
considerConstraintsForTrajectoryOptimization = consider;
@@ -364,7 +407,9 @@ void TrajectoryControllerSubUnit::considerConstraints(bool consider, const Ice::
}
}
-NJointTrajectoryControllerPtr TrajectoryControllerSubUnit::createTrajectoryController(std::vector<std::string>& jointNames, bool deleteIfAlreadyExist)
+NJointTrajectoryControllerPtr
+TrajectoryControllerSubUnit::createTrajectoryController(std::vector<std::string>& jointNames,
+ bool deleteIfAlreadyExist)
{
std::unique_lock lock(controllerMutex);
@@ -396,12 +441,9 @@ NJointTrajectoryControllerPtr TrajectoryControllerSubUnit::createTrajectoryContr
config->considerConstraints = considerConstraintsForTrajectoryOptimization;
config->isPreview = isPreview;
controllerName = this->getName() + "_" + "JointTrajectory" + IceUtil::generateUUID();
- trajController = NJointTrajectoryControllerPtr::dynamicCast(
- robotUnit->createNJointController(
- "NJointTrajectoryController", controllerName,
- config, true, true
- )
- );
+ trajController =
+ NJointTrajectoryControllerPtr::dynamicCast(robotUnit->createNJointController(
+ "NJointTrajectoryController", controllerName, config, true, true));
while (!getArmarXManager()->getIceManager()->isObjectReachable(controllerName))
{
@@ -411,7 +453,8 @@ NJointTrajectoryControllerPtr TrajectoryControllerSubUnit::createTrajectoryContr
return trajController;
}
-void TrajectoryControllerSubUnit::assureTrajectoryController()
+void
+TrajectoryControllerSubUnit::assureTrajectoryController()
{
std::unique_lock lock(controllerMutex);
@@ -422,11 +465,13 @@ void TrajectoryControllerSubUnit::assureTrajectoryController()
ARMARX_CHECK_EXPRESSION(jointTrajController);
}
-bool TrajectoryControllerSubUnit::controllerExists()
+bool
+TrajectoryControllerSubUnit::controllerExists()
{
std::unique_lock lock(controllerMutex);
- auto allNJointControllers = robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
+ auto allNJointControllers =
+ robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
NJointTrajectoryControllerPtr trajController;
for (const auto& controller : allNJointControllers)
{
@@ -444,13 +489,15 @@ bool TrajectoryControllerSubUnit::controllerExists()
return false;
}
-void TrajectoryControllerSubUnit::previewRun()
+void
+TrajectoryControllerSubUnit::previewRun()
{
if (controllerExists())
{
if (jointTrajController->isControllerActive())
{
- std::vector<Ice::DoubleSeq > states = jointTraj->getAllStates(jointTrajController->getCurrentTrajTime(), 1);
+ std::vector<Ice::DoubleSeq> states =
+ jointTraj->getAllStates(jointTrajController->getCurrentTrajTime(), 1);
NameValueMap targetPositionValues;
auto dimNames = jointTraj->getDimensionNames();
@@ -467,15 +514,17 @@ void TrajectoryControllerSubUnit::previewRun()
}
}
-void TrajectoryControllerSubUnit::setup(RobotUnit* rUnit)
+void
+TrajectoryControllerSubUnit::setup(RobotUnit* rUnit)
{
ARMARX_CHECK_EXPRESSION(!robotUnit);
ARMARX_CHECK_EXPRESSION(rUnit);
robotUnit = rUnit;
}
-
-void armarx::TrajectoryControllerSubUnit::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
+void
+armarx::TrajectoryControllerSubUnit::componentPropertiesUpdated(
+ const std::set<std::string>& changedProperties)
{
ARMARX_INFO << "Changning properties";
if (changedProperties.count("Kp"))
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
index 9f48b80609903888caf23163cdefeb1c801ddf74..62c8d33851642c8f462a54526469f05b1340e35e 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
@@ -22,39 +22,61 @@
#pragma once
-#include "RobotUnitSubUnit.h"
+#include <mutex>
#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
+
#include "../NJointControllers/NJointTrajectoryController.h"
#include "../RobotUnit.h"
-
#include "KinematicSubUnit.h"
-
-#include <mutex>
+#include "RobotUnitSubUnit.h"
namespace armarx
{
- class TrajectoryControllerSubUnitPropertyDefinitions:
+ class TrajectoryControllerSubUnitPropertyDefinitions :
public armarx::ComponentPropertyDefinitions
{
public:
- TrajectoryControllerSubUnitPropertyDefinitions(std::string prefix):
+ TrajectoryControllerSubUnitPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("KinematicUnitName", "Name of the KinematicUnit. Only needed for returning to zeroFramePose while resetting.");
- defineOptionalProperty<float>("Kp", 1.f, "Proportional gain value of PID Controller", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<float>("Ki", 0.0f, "Integral gain value of PID Controller", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<float>("Kd", 0.0f, "Differential gain value of PID Controller", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<float>("absMaximumVelocity", 80.0f, "The PID will never set a velocity above this threshold (degree/s)");
- defineOptionalProperty<std::string>("CustomRootNode", "", "If this value is set, the root pose of the motion is set for this node instead of the root joint.");
-
- defineOptionalProperty<bool>("EnableRobotPoseUnit", false, "Specify whether RobotPoseUnit should be used to move the robot's position. Only useful in simulation.");
- defineOptionalProperty<std::string>("RobotPoseUnitName", "RobotPoseUnit", "Name of the RobotPoseUnit to which the robot position should be sent");
+ defineRequiredProperty<std::string>("KinematicUnitName",
+ "Name of the KinematicUnit. Only needed for "
+ "returning to zeroFramePose while resetting.");
+ defineOptionalProperty<float>("Kp",
+ 1.f,
+ "Proportional gain value of PID Controller",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>("Ki",
+ 0.0f,
+ "Integral gain value of PID Controller",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>("Kd",
+ 0.0f,
+ "Differential gain value of PID Controller",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>(
+ "absMaximumVelocity",
+ 80.0f,
+ "The PID will never set a velocity above this threshold (degree/s)");
+ defineOptionalProperty<std::string>("CustomRootNode",
+ "",
+ "If this value is set, the root pose of the motion "
+ "is set for this node instead of the root joint.");
+
+ defineOptionalProperty<bool>("EnableRobotPoseUnit",
+ false,
+ "Specify whether RobotPoseUnit should be used to move the "
+ "robot's position. Only useful in simulation.");
+ defineOptionalProperty<std::string>(
+ "RobotPoseUnitName",
+ "RobotPoseUnit",
+ "Name of the RobotPoseUnit to which the robot position should be sent");
}
};
-
TYPEDEF_PTRS_HANDLE(TrajectoryControllerSubUnit);
+
class TrajectoryControllerSubUnit :
virtual public RobotUnitSubUnit,
virtual public TrajectoryPlayerInterface,
@@ -65,10 +87,13 @@ namespace armarx
bool startTrajectoryPlayer(const Ice::Current& = Ice::emptyCurrent) override;
bool pauseTrajectoryPlayer(const Ice::Current& = Ice::emptyCurrent) override;
bool stopTrajectoryPlayer(const Ice::Current& = Ice::emptyCurrent) override;
- bool resetTrajectoryPlayer(bool moveToFrameZeroPose, const Ice::Current& = Ice::emptyCurrent) override;
+ bool resetTrajectoryPlayer(bool moveToFrameZeroPose,
+ const Ice::Current& = Ice::emptyCurrent) override;
- void loadJointTraj(const TrajectoryBasePtr& jointTraj, const Ice::Current& = Ice::emptyCurrent) override;
- void loadBasePoseTraj(const TrajectoryBasePtr& basePoseTraj, const Ice::Current& = Ice::emptyCurrent) override;
+ void loadJointTraj(const TrajectoryBasePtr& jointTraj,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void loadBasePoseTraj(const TrajectoryBasePtr& basePoseTraj,
+ const Ice::Current& = Ice::emptyCurrent) override;
void setLoopPlayback(bool loop, const Ice::Current& = Ice::emptyCurrent) override;
Ice::Double getEndTime(const Ice::Current& = Ice::emptyCurrent) override;
@@ -77,10 +102,14 @@ namespace armarx
void setEndTime(Ice::Double, const Ice::Current& = Ice::emptyCurrent) override;
// Within the RobotUnit the NJointTrajectoryController is always in VelocityControl
- void setIsVelocityControl(bool, const Ice::Current& = Ice::emptyCurrent) override {}
+ void
+ setIsVelocityControl(bool, const Ice::Current& = Ice::emptyCurrent) override
+ {
+ }
void setIsPreview(bool, const Ice::Current& = Ice::emptyCurrent) override;
- bool setJointsInUse(const std::string&, bool, const Ice::Current& = Ice::emptyCurrent) override;
+ bool
+ setJointsInUse(const std::string&, bool, const Ice::Current& = Ice::emptyCurrent) override;
void enableRobotPoseUnit(bool, const Ice::Current& = Ice::emptyCurrent) override;
void considerConstraints(bool, const Ice::Current& = Ice::emptyCurrent) override;
@@ -91,39 +120,82 @@ namespace armarx
void setup(RobotUnit* rUnit);
// KinematicUnitListener interface
- void reportControlModeChanged(const NameControlModeMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointAngles(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointVelocities(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointTorques(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointAccelerations(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointCurrents(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointMotorTemperatures(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&) override {}
+ void
+ reportControlModeChanged(const NameControlModeMap&,
+ Ice::Long,
+ bool,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointAngles(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointVelocities(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
- void setOffset(const ::Eigen::Matrix4f&, const ::Ice::Current& = ::Ice::emptyCurrent) override {}
+ void
+ reportJointTorques(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointAccelerations(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointCurrents(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointMotorTemperatures(const NameValueMap&,
+ Ice::Long,
+ bool,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ setOffset(const ::Eigen::Matrix4f&, const ::Ice::Current& = ::Ice::emptyCurrent) override
+ {
+ }
// ManagedIceObject interface
protected:
void onInitComponent() override;
void onConnectComponent() override;
void onDisconnectComponent() override;
- std::string getDefaultName() const override
+
+ std::string
+ getDefaultName() const override
{
return "TrajectoryPlayer";
}
+
/**
* @see PropertyUser::createPropertyDefinitions()
*/
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
+ armarx::PropertyDefinitionsPtr
+ createPropertyDefinitions() override
{
- return armarx::PropertyDefinitionsPtr
- {
- new TrajectoryControllerSubUnitPropertyDefinitions{getConfigIdentifier()}
- };
+ return armarx::PropertyDefinitionsPtr{
+ new TrajectoryControllerSubUnitPropertyDefinitions{getConfigIdentifier()}};
}
private:
- NJointTrajectoryControllerPtr createTrajectoryController(std::vector<std::string>& jointNames, bool deleteIfAlreadyExist);
+ NJointTrajectoryControllerPtr
+ createTrajectoryController(std::vector<std::string>& jointNames, bool deleteIfAlreadyExist);
void assureTrajectoryController();
bool controllerExists();
void previewRun();
@@ -163,6 +235,7 @@ namespace armarx
// Component interface
public:
- virtual void componentPropertiesUpdated(const std::set<std::string>& changedProperties) override;
+ virtual void
+ componentPropertiesUpdated(const std::set<std::string>& changedProperties) override;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp b/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp
index 23101df79925c5bcca01d070d043f91bcf2e1a43..f02cd9172e2d368cf3877d92e60cdb736000ba3a 100644
--- a/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp
@@ -23,15 +23,19 @@
#define BOOST_TEST_MODULE RobotAPI::BasicControllers::Test
#define ARMARX_BOOST_TEST
#define CREATE_LOGFILES
-#include <random>
-#include <iostream>
#include <filesystem>
-#include "../util/CtrlUtil.h"
+#include <iostream>
+#include <random>
+
#include <boost/algorithm/clamp.hpp>
+
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/algorithm.h>
+
#include <RobotAPI/Test.h>
+
#include "../BasicControllers.h"
+#include "../util/CtrlUtil.h"
using namespace armarx;
//params for random tests
const std::size_t tries = 1;
@@ -41,17 +45,16 @@ const std::size_t ticks = 5000; // each tick is 0.75 to 1.25 ms
#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 std::filesystem::path fpath
-{
- "controller_logfiles/"
-};
+#include <boost/filesystem.hpp>
+
+static const std::filesystem::path fpath{"controller_logfiles/"};
static std::ofstream f;
static bool isSetup = false;
-void change_logging_file(const std::string& name)
+void
+change_logging_file(const std::string& name)
{
if (f.is_open())
{
@@ -73,19 +76,26 @@ void change_logging_file(const std::string& name)
}
#else
-#define LOG_CONTROLLER_DATA_WRITE_TO(name) do{}while(0)
-#define LOG_CONTROLLER_DATA(...) do{}while(0)
+#define LOG_CONTROLLER_DATA_WRITE_TO(name) \
+ do \
+ { \
+ } while (0)
+#define LOG_CONTROLLER_DATA(...) \
+ do \
+ { \
+ } while (0)
#endif
-unsigned int getSeed()
+unsigned int
+getSeed()
{
- static const auto seed = std::random_device {}();
+ static const auto seed = std::random_device{}();
std::cout << "seed = " << seed << std::endl;
return seed;
}
-static std::mt19937 gen {getSeed()};
+static std::mt19937 gen{getSeed()};
struct Simulation
{
@@ -122,7 +132,8 @@ struct Simulation
std::uniform_real_distribution<double> pDist;
std::uniform_real_distribution<double> tDist;
- void reset()
+ void
+ reset()
{
time = 0;
@@ -143,22 +154,24 @@ struct Simulation
brakingDist = 0;
posAfterBraking = 0;
- vDist = std::uniform_real_distribution<double> { -maxvel, maxvel};
- aDist = std::uniform_real_distribution<double> {maxvel / 4, maxvel * 4 };
- tDist = std::uniform_real_distribution<double> {maxDt * 0.75f, maxDt * 1.25f};
- pDist = std::uniform_real_distribution<double> {posLoHard, posHiHard};
+ vDist = std::uniform_real_distribution<double>{-maxvel, maxvel};
+ aDist = std::uniform_real_distribution<double>{maxvel / 4, maxvel * 4};
+ tDist = std::uniform_real_distribution<double>{maxDt * 0.75f, maxDt * 1.25f};
+ pDist = std::uniform_real_distribution<double>{posLoHard, posHiHard};
}
//updating
- template<class FNC>
- void tick(FNC& callee)
+ template <class FNC>
+ void
+ tick(FNC& callee)
{
- dt = maxDt;//tDist(gen);
+ dt = maxDt; //tDist(gen);
callee();
log();
}
- void updateVel(double newvel)
+ void
+ updateVel(double newvel)
{
BOOST_CHECK(std::isfinite(newvel));
//save old
@@ -169,14 +182,16 @@ struct Simulation
//update
curvel = newvel;
curacc = (curvel - oldvel) / dt;
- jerk = (curacc - oldacc) / dt;
- curpos += ctrlutil::s(dt, 0, curvel, curacc, 0/*math::MathUtils::Sign(curacc-oldacc) * jerk*/);
+ jerk = (curacc - oldacc) / dt;
+ curpos +=
+ ctrlutil::s(dt, 0, curvel, curacc, 0 /*math::MathUtils::Sign(curacc-oldacc) * jerk*/);
time += dt;
brakingDist = brakingDistance(curvel, dec);
posAfterBraking = curpos + brakingDist;
}
- void updatePos(double newPos)
+ void
+ updatePos(double newPos)
{
BOOST_CHECK(std::isfinite(newPos));
//save old
@@ -187,7 +202,7 @@ struct Simulation
//update
curvel = (newPos - oldpos) / dt;
curacc = (curvel - oldvel) / dt;
- jerk = (curacc - oldacc) / dt;
+ jerk = (curacc - oldacc) / dt;
curpos = newPos;
time += dt;
brakingDist = brakingDistance(curvel, dec);
@@ -195,20 +210,23 @@ struct Simulation
}
//checks
- void checkVel()
+ void
+ checkVel()
{
//check v
BOOST_CHECK_LE(curvel, maxvel * 1.01);
BOOST_CHECK_LE(-maxvel * 1.01, curvel);
}
- void checkPos()
+ void
+ checkPos()
{
BOOST_CHECK_LE(curpos, posHiHard);
BOOST_CHECK_LE(posLoHard, curpos);
}
- void checkAcc()
+ void
+ checkAcc()
{
if (sign(curvel) != sign(oldvel))
{
@@ -221,8 +239,10 @@ struct Simulation
//we decelerated
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";
+ 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);
}
@@ -231,21 +251,27 @@ struct Simulation
//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";
+ 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)
+ void
+ writeHeader(const std::string& name)
{
LOG_CONTROLLER_DATA_WRITE_TO(name);
- LOG_CONTROLLER_DATA("time curpos targpos posHiHard posLoHard posHi posLo curvel targvel maxv curacc acc dec brakingDist posAfterBraking");
+ LOG_CONTROLLER_DATA("time curpos targpos posHiHard posLoHard posHi posLo curvel targvel "
+ "maxv curacc acc dec brakingDist posAfterBraking");
reset();
}
- void log()
+
+ void
+ log()
{
// //output a neg val for dec and a pos val for acc
// double outputacc;
@@ -263,11 +289,10 @@ struct Simulation
// outputacc *= -1;
// }
// }
- LOG_CONTROLLER_DATA(time << " " <<
- curpos << " " << targpos << " " << posHiHard << " " << posLoHard << " " << posHi << " " << posLo << " " <<
- curvel << " " << targvel << " " << maxvel << " " <<
- curacc << " " << acc << " " << -dec << " " <<
- brakingDist << " " << posAfterBraking);
+ LOG_CONTROLLER_DATA(time << " " << curpos << " " << targpos << " " << posHiHard << " "
+ << posLoHard << " " << posHi << " " << posLo << " " << curvel
+ << " " << targvel << " " << maxvel << " " << curacc << " " << acc
+ << " " << -dec << " " << brakingDist << " " << posAfterBraking);
}
};
@@ -279,8 +304,8 @@ BOOST_AUTO_TEST_CASE(VelocityControllerWithRampedAccelerationAndPositionBoundsTe
// s.posHi = 0;
// s.posLo = 0;
- auto distPosLo = std::uniform_real_distribution<double> { -M_PI, -M_PI_4};
- auto distPosHi = std::uniform_real_distribution<double> { M_PI_4, M_PI};
+ auto distPosLo = std::uniform_real_distribution<double>{-M_PI, -M_PI_4};
+ auto distPosHi = std::uniform_real_distribution<double>{M_PI_4, M_PI};
s.posLo = distPosLo(gen);
s.posHi = distPosHi(gen);
// double p = 20.5;
@@ -323,19 +348,20 @@ BOOST_AUTO_TEST_CASE(VelocityControllerWithRampedAccelerationAndPositionBoundsTe
};
std::cout << "random tests\n";
- for (std::size_t try_ = 0; try_ < tries; ++ try_)
+ for (std::size_t try_ = 0; try_ < tries; ++try_)
{
ctrl = VelocityControllerWithRampedAccelerationAndPositionBounds();
s.writeHeader("RampedAccPositionLimitedVelCtrl+acc_random_" + to_string(try_));
- s.maxvel = 5;//std::abs(s.vDist(gen)) + 1;
- s.curvel = 5;//armarx::math::MathUtils::LimitTo(s.vDist(gen), s.maxvel);
+ s.maxvel = 5; //std::abs(s.vDist(gen)) + 1;
+ s.curvel = 5; //armarx::math::MathUtils::LimitTo(s.vDist(gen), s.maxvel);
ctrl.currentV = s.curvel;
s.curpos = s.pDist(gen);
ctrl.currentAcc = s.pDist(gen);
s.curacc = ctrl.currentAcc;
- ARMARX_INFO << VAROUT(s.dt) << VAROUT(s.curpos) << VAROUT(s.targvel) << " " << VAROUT(s.acc) << VAROUT(s.dec) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
+ ARMARX_INFO << VAROUT(s.dt) << VAROUT(s.curpos) << VAROUT(s.targvel) << " " << VAROUT(s.acc)
+ << VAROUT(s.dec) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
// p = ((std::min(s.acc,s.dec)*s.maxDt*0.75-pControlVelLimit)/pControlPosErrorLimit) * 0.99f; // sometimes <0
- ctrl.jerk = 100;//(random() % 100) + 10;
+ ctrl.jerk = 100; //(random() % 100) + 10;
s.log();
double stopTime = -1;
for (std::size_t tick = 0; tick < ticks; ++tick)
@@ -344,17 +370,23 @@ BOOST_AUTO_TEST_CASE(VelocityControllerWithRampedAccelerationAndPositionBoundsTe
// s.targpos = tick > 900 ? 3 : 5;
s.tick(testTick);
- ARMARX_INFO << /*deactivateSpam(0.01) <<*/ VAROUT(s.dt * tick) << VAROUT(s.curpos) << VAROUT(s.targvel) << VAROUT(s.curacc) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
+ ARMARX_INFO << /*deactivateSpam(0.01) <<*/ VAROUT(s.dt * tick) << VAROUT(s.curpos)
+ << VAROUT(s.targvel) << VAROUT(s.curacc) << VAROUT(s.maxvel)
+ << VAROUT(s.curvel) << VAROUT(s.jerk);
if (std::abs(s.curvel - s.targvel) < 0.001 && stopTime < 0)
{
- stopTime = s.dt * (tick + 20); // let it run for some more ticks to make sure the velocity is stable
+ stopTime =
+ s.dt *
+ (tick +
+ 20); // let it run for some more ticks to make sure the velocity is stable
}
if (stopTime > 0 && s.dt * tick > stopTime)
{
break;
}
}
- BOOST_CHECK_LE(std::abs(s.curvel - s.targvel), 0.001); // allow error of 0.5729577951308232 deg
+ BOOST_CHECK_LE(std::abs(s.curvel - s.targvel),
+ 0.001); // allow error of 0.5729577951308232 deg
}
std::cout << "bounds tests\n";
std::cout << "TODO\n";
@@ -407,16 +439,17 @@ BOOST_AUTO_TEST_CASE(VelocityControllerWithRampedAccelerationTest)
};
std::cout << "random tests\n";
- for (std::size_t try_ = 0; try_ < tries; ++ try_)
+ for (std::size_t try_ = 0; try_ < tries; ++try_)
{
ctrl = VelocityControllerWithRampedAcceleration();
s.writeHeader("RampedAccVelCtrl+acc_random_" + to_string(try_));
s.maxvel = std::abs(s.vDist(gen)) + 1;
s.curvel = armarx::math::MathUtils::LimitTo(s.vDist(gen), s.maxvel);
ctrl.currentV = s.curvel;
- s.curpos = 1;//s.pDist(gen);
+ s.curpos = 1; //s.pDist(gen);
ctrl.currentAcc = s.pDist(gen);
- ARMARX_INFO << VAROUT(s.dt) << VAROUT(s.curpos) << VAROUT(s.targvel) << " " << VAROUT(s.acc) << VAROUT(s.dec) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
+ ARMARX_INFO << VAROUT(s.dt) << VAROUT(s.curpos) << VAROUT(s.targvel) << " " << VAROUT(s.acc)
+ << VAROUT(s.dec) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
// p = ((std::min(s.acc,s.dec)*s.maxDt*0.75-pControlVelLimit)/pControlPosErrorLimit) * 0.99f; // sometimes <0
ctrl.jerk = (random() % 100) + 10;
s.log();
@@ -430,14 +463,18 @@ BOOST_AUTO_TEST_CASE(VelocityControllerWithRampedAccelerationTest)
// ARMARX_INFO << /*deactivateSpam(0.01) <<*/ VAROUT(s.dt * tick) << VAROUT(s.curpos) << VAROUT(s.targvel) << VAROUT(s.curacc) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
if (std::abs(s.curvel - s.targvel) < 0.001 && stopTime < 0)
{
- stopTime = s.dt * (tick + 20); // let it run for some more ticks to make sure the velocity is stable
+ stopTime =
+ s.dt *
+ (tick +
+ 20); // let it run for some more ticks to make sure the velocity is stable
}
if (stopTime > 0 && s.dt * tick > stopTime)
{
break;
}
}
- BOOST_CHECK_LE(std::abs(s.curvel - s.targvel), 0.001); // allow error of 0.5729577951308232 deg
+ BOOST_CHECK_LE(std::abs(s.curvel - s.targvel),
+ 0.001); // allow error of 0.5729577951308232 deg
}
std::cout << "bounds tests\n";
std::cout << "TODO\n";
@@ -446,7 +483,6 @@ BOOST_AUTO_TEST_CASE(VelocityControllerWithRampedAccelerationTest)
std::cout << "done positionThroughVelocityControlWithAccelerationBounds \n";
}
-
//BOOST_AUTO_TEST_CASE(velocityControlWithAccelerationBoundsTest)
//{
// std::cout << "starting velocityControlWithAccelerationBoundsTest \n";
@@ -696,7 +732,7 @@ BOOST_AUTO_TEST_CASE(MinJerkPositionControllerTest)
};
std::cout << "random tests\n";
- for (std::size_t try_ = 0; try_ < tries; ++ try_)
+ for (std::size_t try_ = 0; try_ < tries; ++try_)
{
ctrl = MinJerkPositionController();
s.writeHeader("minJerkPosCtrl+acc_random_" + to_string(try_));
@@ -706,8 +742,10 @@ BOOST_AUTO_TEST_CASE(MinJerkPositionControllerTest)
s.dec = s.aDist(gen);
ctrl.currentAcc = s.curacc;
ctrl.currentV = s.curvel;
- s.curpos = 1;//s.pDist(gen);
- ARMARX_INFO << VAROUT(s.dt) << VAROUT(s.curpos) << "TargetPos: " << s.targpos << " " << VAROUT(s.acc) << VAROUT(s.dec) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
+ s.curpos = 1; //s.pDist(gen);
+ ARMARX_INFO << VAROUT(s.dt) << VAROUT(s.curpos) << "TargetPos: " << s.targpos << " "
+ << VAROUT(s.acc) << VAROUT(s.dec) << VAROUT(s.maxvel) << VAROUT(s.curvel)
+ << VAROUT(s.jerk);
// p = ((std::min(s.acc,s.dec)*s.maxDt*0.75-pControlVelLimit)/pControlPosErrorLimit) * 0.99f; // sometimes <0
s.log();
double stopTime = -1;
@@ -720,7 +758,10 @@ BOOST_AUTO_TEST_CASE(MinJerkPositionControllerTest)
// ARMARX_INFO << /*deactivateSpam(0.01) <<*/ VAROUT(s.dt * tick) << VAROUT(s.curpos) << "TargetPos: " << s.targpos << " " << VAROUT(s.curacc) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
if (std::abs(s.curpos - s.targpos) < 0.01 && stopTime < 0)
{
- stopTime = s.dt * (tick + 20); // let it run for some more ticks to make sure the velocity is stable
+ stopTime =
+ s.dt *
+ (tick +
+ 20); // let it run for some more ticks to make sure the velocity is stable
ARMARX_INFO << VAROUT(s.time) << VAROUT(stopTime);
}
if (stopTime > 0 && s.dt * tick > stopTime)
@@ -728,7 +769,8 @@ BOOST_AUTO_TEST_CASE(MinJerkPositionControllerTest)
break;
}
}
- BOOST_CHECK_LE(std::abs(s.curpos - s.targpos), 0.01); // allow error of 0.5729577951308232 deg
+ 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";
@@ -737,8 +779,6 @@ BOOST_AUTO_TEST_CASE(MinJerkPositionControllerTest)
std::cout << "done positionThroughVelocityControlWithAccelerationBounds \n";
}
-
-
//BOOST_AUTO_TEST_CASE(positionThroughVelocityControlWithAccelerationBoundsTest)
//{
// return;
@@ -947,4 +987,3 @@ BOOST_AUTO_TEST_CASE(MinJerkPositionControllerTest)
//// std::cout << "done positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPosition \n";
////}
-
diff --git a/source/RobotAPI/components/units/RobotUnit/test/SingleBasicControllerTest.cpp b/source/RobotAPI/components/units/RobotUnit/test/SingleBasicControllerTest.cpp
index ec56bf0511be737f3b46b48d2adc7c4b3a703a40..c34b618afcd0cf9b03c4fae663866a52a1a11752 100644
--- a/source/RobotAPI/components/units/RobotUnit/test/SingleBasicControllerTest.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/test/SingleBasicControllerTest.cpp
@@ -23,14 +23,16 @@
#define BOOST_TEST_MODULE RobotAPI::BasicControllers::Test
#define ARMARX_BOOST_TEST
#define CREATE_LOGFILES
-#include <random>
#include <iostream>
+#include <random>
#include <boost/algorithm/clamp.hpp>
-#include <ArmarXCore/core/util/algorithm.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/util/algorithm.h>
+
#include <RobotAPI/Test.h>
+
#include "../BasicControllers.h"
using namespace armarx;
//params for random tests
@@ -41,17 +43,16 @@ const std::size_t ticks = 20000; // each tick is 0.75 to 1.25 ms
#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/"
-};
+#include <boost/filesystem.hpp>
+
+static const boost::filesystem::path fpath{"controller_logfiles/"};
static std::ofstream f;
static bool isSetup = false;
-void change_logging_file(const std::string& name)
+void
+change_logging_file(const std::string& name)
{
if (f.is_open())
{
@@ -170,19 +171,26 @@ void change_logging_file(const std::string& name)
}
#else
-#define LOG_CONTROLLER_DATA_WRITE_TO(name) do{}while(0)
-#define LOG_CONTROLLER_DATA(...) do{}while(0)
+#define LOG_CONTROLLER_DATA_WRITE_TO(name) \
+ do \
+ { \
+ } while (0)
+#define LOG_CONTROLLER_DATA(...) \
+ do \
+ { \
+ } while (0)
#endif
-unsigned int getSeed()
+unsigned int
+getSeed()
{
- static const auto seed = std::random_device {}();
+ static const auto seed = std::random_device{}();
std::cout << "seed = " << seed << std::endl;
return seed;
}
-static std::mt19937 gen {getSeed()};
+static std::mt19937 gen{getSeed()};
struct Simulation
{
@@ -217,7 +225,8 @@ struct Simulation
std::uniform_real_distribution<float> pDist;
std::uniform_real_distribution<float> tDist;
- void reset()
+ void
+ reset()
{
time = 0;
@@ -237,22 +246,24 @@ struct Simulation
brakingDist = 0;
posAfterBraking = 0;
- vDist = std::uniform_real_distribution<float> { -maxvel, maxvel};
- aDist = std::uniform_real_distribution<float> {maxvel, maxvel * 4 };
- tDist = std::uniform_real_distribution<float> {maxDt * 0.75f, maxDt * 1.25f};
- pDist = std::uniform_real_distribution<float> {posLoHard, posHiHard};
+ vDist = std::uniform_real_distribution<float>{-maxvel, maxvel};
+ aDist = std::uniform_real_distribution<float>{maxvel, 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)
+ template <class FNC>
+ void
+ tick(FNC& callee)
{
dt = tDist(gen);
callee();
log();
}
- void updateVel(float newvel)
+ void
+ updateVel(float newvel)
{
BOOST_CHECK(std::isfinite(newvel));
//save old
@@ -270,20 +281,23 @@ struct Simulation
}
//checks
- void checkVel()
+ void
+ checkVel()
{
//check v
BOOST_CHECK_LE(curvel, maxvel * 1.01);
BOOST_CHECK_LE(-maxvel * 1.01, curvel);
}
- void checkPos()
+ void
+ checkPos()
{
BOOST_CHECK_LE(curpos, posHiHard);
BOOST_CHECK_LE(posLoHard, curpos);
}
- void checkAcc()
+ void
+ checkAcc()
{
if (sign(curvel) != sign(oldvel))
{
@@ -306,21 +320,27 @@ struct Simulation
//we accellerated
if (!(curacc < acc * 1.01))
{
- ARMARX_INFO << "Time[" << time << "] violated deceleration bound! vold " << oldvel << " / vnew " << curvel << " / dv " << std::abs(oldvel - curvel) << " / dt " << dt << " / acc " << acc
- << " / (targetv " << targvel << ")\n";
+ ARMARX_INFO << "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)
+ 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");
+ LOG_CONTROLLER_DATA("time curpos targpos posHiHard posLoHard posHi posLo curvel targvel "
+ "maxvel curacc acc dec brakingDist posAfterBraking");
reset();
}
- void log()
+
+ void
+ log()
{
//output a neg val for dec and a pos val for acc
float outputacc;
@@ -338,16 +358,14 @@ struct Simulation
outputacc *= -1;
}
}
- LOG_CONTROLLER_DATA(time << " " <<
- curpos << " " << targpos << " " << posHiHard << " " << posLoHard << " " << posHi << " " << posLo << " " <<
- curvel << " " << targvel << " " << maxvel << " " <<
- outputacc << " " << acc << " " << -dec << " " <<
- brakingDist << " " << posAfterBraking);
+ LOG_CONTROLLER_DATA(time << " " << curpos << " " << targpos << " " << posHiHard << " "
+ << posLoHard << " " << posHi << " " << posLo << " " << curvel
+ << " " << targvel << " " << maxvel << " " << outputacc << " "
+ << acc << " " << -dec << " " << brakingDist << " "
+ << posAfterBraking);
}
};
-
-
BOOST_AUTO_TEST_CASE(positionThroughVelocityControlWithAccelerationBoundsTest)
{
std::cout << "starting positionThroughVelocityControlWithAccelerationBounds \n";
@@ -390,7 +408,7 @@ BOOST_AUTO_TEST_CASE(positionThroughVelocityControlWithAccelerationBoundsTest)
};
std::cout << "random tests\n";
- for (std::size_t try_ = 0; try_ < tries; ++ try_)
+ for (std::size_t try_ = 0; try_ < tries; ++try_)
{
s.writeHeader("posViaVelCtrl+acc_random_" + to_string(try_));
s.maxvel = std::abs(s.vDist(gen)) + 1;
@@ -401,7 +419,8 @@ BOOST_AUTO_TEST_CASE(positionThroughVelocityControlWithAccelerationBoundsTest)
s.dec = s.aDist(gen);
//s.acc: 6.08172 s.dec: 7.98634 s.maxvel: 1.73312
//s.curpos: -1.90146 TargetPos: 1.2378s.acc: 5.93058 s.dec: 3.06262 s.maxvel: 2.94756
- ARMARX_INFO << VAROUT(s.curpos) << "TargetPos: " << s.targpos << VAROUT(s.acc) << VAROUT(s.dec) << VAROUT(s.maxvel) << VAROUT(s.curvel);
+ ARMARX_INFO << VAROUT(s.curpos) << "TargetPos: " << s.targpos << VAROUT(s.acc)
+ << VAROUT(s.dec) << VAROUT(s.maxvel) << VAROUT(s.curvel);
// 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)
@@ -440,9 +459,20 @@ BOOST_AUTO_TEST_CASE(timeEstimationTest)
float est = c.estimateTime();
ARMARX_INFO << "estimated time " << est;
float newDt = est + (float)(rand() % 100) / 10;
- findVelocityAndAccelerationForTimeAndDistance(c.targetPosition - c.currentPosition, c.currentV, c.maxV, c.deceleration,
- trapeze(c.currentV, c.acceleration, c.maxV, c.deceleration, 0, c.targetPosition - c.currentPosition),
- newDt, c.maxV, c.acceleration, c.deceleration);
+ findVelocityAndAccelerationForTimeAndDistance(c.targetPosition - c.currentPosition,
+ c.currentV,
+ c.maxV,
+ c.deceleration,
+ trapeze(c.currentV,
+ c.acceleration,
+ c.maxV,
+ c.deceleration,
+ 0,
+ c.targetPosition - c.currentPosition),
+ newDt,
+ c.maxV,
+ c.acceleration,
+ c.deceleration);
// c.maxV = 0.583;
// c.acceleration = 0.16666;
@@ -452,6 +482,4 @@ BOOST_AUTO_TEST_CASE(timeEstimationTest)
ARMARX_INFO << "desired time: " << newDt << " estimated time " << est;
BOOST_CHECK_CLOSE(est, newDt, 1);
}
-
}
-
diff --git a/source/RobotAPI/components/units/RobotUnit/util.cpp b/source/RobotAPI/components/units/RobotUnit/util.cpp
index 903217aae47729f9a77a02ab1697c3bb727dd57b..ecae915c8f06bf7d0288c033cb3f18c305370ed9 100644
--- a/source/RobotAPI/components/units/RobotUnit/util.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util.cpp
@@ -20,4 +20,3 @@
* GNU General Public License
*/
#include "util.h"
-
diff --git a/source/RobotAPI/components/units/RobotUnit/util.h b/source/RobotAPI/components/units/RobotUnit/util.h
index ef3e5c3267124949b39a931e7f1536659eda27e1..1ff7af0703435a2c67193a9dc388dce1adc98c7f 100644
--- a/source/RobotAPI/components/units/RobotUnit/util.h
+++ b/source/RobotAPI/components/units/RobotUnit/util.h
@@ -21,15 +21,17 @@
*/
#pragma once
-#include <ArmarXCore/core/util/PropagateConst.h>
+#include <type_traits>
+
#include <ArmarXCore/core/ManagedIceObject.h>
+#include <ArmarXCore/core/util/PropagateConst.h>
+#include "util/AtomicWrapper.h"
#include "util/EigenForwardDeclarations.h"
#include "util/KeyValueVector.h"
-#include "util/AtomicWrapper.h"
#include "util/Time.h"
#include "util/introspection/ClassMemberInfo.h"
-#include <type_traits>
-
-namespace armarx::DeviceTags {}
+namespace armarx::DeviceTags
+{
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/util/AtomicWrapper.h b/source/RobotAPI/components/units/RobotUnit/util/AtomicWrapper.h
index 639e9efb425ff7109ef25f01ff28eefa10fc5a53..c355cb162edd52b48cefab36106dd6bddbf6064b 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/AtomicWrapper.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/AtomicWrapper.h
@@ -22,6 +22,7 @@
#pragma once
#include <atomic>
+
namespace armarx
{
template <typename T>
@@ -29,26 +30,45 @@ namespace armarx
{
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() : 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)
+
+ AtomicWrapper&
+ operator=(const T& v)
{
atom.store(v);
return *this;
}
- AtomicWrapper& operator=(const std::atomic<T>& a)
+
+ AtomicWrapper&
+ operator=(const std::atomic<T>& a)
{
atom.store(a.load());
return *this;
}
- AtomicWrapper& operator=(const AtomicWrapper& other)
+
+ AtomicWrapper&
+ operator=(const AtomicWrapper& other)
{
atom.store(other.atom.load());
return *this;
}
+
AtomicWrapper& operator=(AtomicWrapper&&) = default;
operator T() const
@@ -56,4 +76,4 @@ namespace armarx
return atom.load();
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
index 4cbdf1e4b7fcde010ea07db88f4d8585163cf2d9..15dc03f356cc2d803f7e5a3bf91b716671ccc7d7 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
@@ -22,26 +22,24 @@
#pragma once
-#include "HeterogenousContinuousContainer.h"
-
-#include "../SensorValues/SensorValueBase.h"
-#include "../ControlTargets/ControlTargetBase.h"
-
-#include "../Devices/SensorDevice.h"
-#include "../Devices/ControlDevice.h"
+#include <vector>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/util/StringHelperTemplates.h>
#include <ArmarXCore/core/util/PropagateConst.h>
+#include <ArmarXCore/core/util/StringHelperTemplates.h>
#include <ArmarXCore/core/util/TripleBuffer.h>
-#include <vector>
+#include "../ControlTargets/ControlTargetBase.h"
+#include "../Devices/ControlDevice.h"
+#include "../Devices/SensorDevice.h"
+#include "../SensorValues/SensorValueBase.h"
+#include "HeterogenousContinuousContainer.h"
namespace armarx
{
class RobotUnit;
struct ControlThreadOutputBuffer;
-}
+} // namespace armarx
namespace armarx::RobotUnitModule
{
@@ -52,9 +50,10 @@ namespace armarx::detail
{
struct RtMessageLogEntryBase
{
- RtMessageLogEntryBase():
- time {IceUtil::Time::now()}
- {}
+ RtMessageLogEntryBase() : time{IceUtil::Time::now()}
+ {
+ }
+
virtual ~RtMessageLogEntryBase() = default;
RtMessageLogEntryBase& setLoggingLevel(MessageTypeT lvl);
@@ -71,10 +70,11 @@ namespace armarx::detail
ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER_BASE(RtMessageLogEntryBase)
protected:
friend struct RtMessageLogBuffer;
+
private:
- MessageTypeT loggingLevel {MessageTypeT::UNDEFINED};
- float deactivateSpamSec {0};
- bool printMsg {false};
+ MessageTypeT loggingLevel{MessageTypeT::UNDEFINED};
+ float deactivateSpamSec{0};
+ bool printMsg{false};
std::uint64_t deactivateSpamTag_;
IceUtil::Time time;
};
@@ -82,6 +82,7 @@ namespace armarx::detail
struct RtMessageLogEntryDummy : RtMessageLogEntryBase
{
static RtMessageLogEntryDummy Instance;
+
protected:
std::string format() const final override;
std::size_t line() const final override;
@@ -94,20 +95,28 @@ namespace armarx::detail
struct RtMessageLogEntryNull : RtMessageLogEntryBase
{
static RtMessageLogEntryNull Instance;
+
protected:
- std::string format() const final override
+ std::string
+ format() const final override
{
return "";
}
- std::size_t line() const final override
+
+ std::size_t
+ line() const final override
{
return 0;
}
- std::string file() const final override
+
+ std::string
+ file() const final override
{
return "";
}
- std::string func() const final override
+
+ std::string
+ func() const final override
{
return "";
}
@@ -118,9 +127,10 @@ namespace armarx::detail
struct RtMessageLogBuffer
{
RtMessageLogBuffer(const RtMessageLogBuffer& other, bool minimize = false);
- RtMessageLogBuffer(
- std::size_t bufferSize, std::size_t numEntries,
- std::size_t bufferMaxSize, std::size_t bufferMaxNumberEntries);
+ RtMessageLogBuffer(std::size_t bufferSize,
+ std::size_t numEntries,
+ std::size_t bufferMaxSize,
+ std::size_t bufferMaxNumberEntries);
~RtMessageLogBuffer();
RtMessageLogBuffer() = delete;
@@ -134,6 +144,7 @@ namespace armarx::detail
std::size_t getMaximalBufferSize() const;
std::size_t getMaximalNumberOfBufferEntries() const;
+
private:
void deleteAll();
@@ -152,27 +163,28 @@ namespace armarx::detail
void* bufferPlace;
std::vector<const RtMessageLogEntryBase*> entries;
- std::size_t entriesWritten {0};
+ std::size_t entriesWritten{0};
- std::size_t requiredAdditionalBufferSpace {0};
- std::size_t requiredAdditionalEntries {0};
- std::size_t messagesLost {0};
+ std::size_t requiredAdditionalBufferSpace{0};
+ std::size_t requiredAdditionalEntries{0};
+ std::size_t messagesLost{0};
- std::size_t maxAlign {1};
+ std::size_t maxAlign{1};
};
-
struct ControlThreadOutputBufferEntry
{
//default ctors / ops
ControlThreadOutputBufferEntry(
const KeyValueVector<std::string, ControlDevicePtr>& controlDevices,
- const KeyValueVector<std::string, SensorDevicePtr >& sensorDevices,
- std::size_t messageBufferSize, std::size_t messageBufferNumberEntries,
- std::size_t messageBufferMaxSize, std::size_t messageBufferMaxNumberEntries
- );
- ControlThreadOutputBufferEntry(const ControlThreadOutputBufferEntry& other, bool minimize = false);
+ const KeyValueVector<std::string, SensorDevicePtr>& sensorDevices,
+ std::size_t messageBufferSize,
+ std::size_t messageBufferNumberEntries,
+ std::size_t messageBufferMaxSize,
+ std::size_t messageBufferMaxNumberEntries);
+ ControlThreadOutputBufferEntry(const ControlThreadOutputBufferEntry& other,
+ bool minimize = false);
ControlThreadOutputBufferEntry() = delete;
ControlThreadOutputBufferEntry(ControlThreadOutputBufferEntry&&) = delete;
@@ -188,13 +200,14 @@ namespace armarx::detail
IceUtil::Time timeSinceLastIteration;
std::vector<PropagateConst<SensorValueBase*>> sensors;
std::vector<std::vector<PropagateConst<ControlTargetBase*>>> control;
- std::size_t iteration {0};
+ std::size_t iteration{0};
RtMessageLogBuffer messages;
+
private:
std::vector<std::uint8_t> buffer;
};
-}
+} // namespace armarx::detail
namespace armarx
{
@@ -202,17 +215,19 @@ namespace armarx
{
using Entry = detail::ControlThreadOutputBufferEntry;
using RtMessageLogEntryBase = detail::RtMessageLogEntryBase;
- using ConsumerFunctor = std::function<void (const Entry&, std::size_t, std::size_t)>;
- std::size_t initialize(
- std::size_t numEntries,
- const KeyValueVector<std::string, ControlDevicePtr>& controlDevices,
- const KeyValueVector<std::string, SensorDevicePtr >& sensorDevices,
- std::size_t messageBufferSize, std::size_t messageBufferNumberEntries,
- std::size_t messageBufferMaxSize, std::size_t messageBufferMaxNumberEntries);
+ using ConsumerFunctor = std::function<void(const Entry&, std::size_t, std::size_t)>;
+ std::size_t initialize(std::size_t numEntries,
+ const KeyValueVector<std::string, ControlDevicePtr>& controlDevices,
+ const KeyValueVector<std::string, SensorDevicePtr>& sensorDevices,
+ std::size_t messageBufferSize,
+ std::size_t messageBufferNumberEntries,
+ std::size_t messageBufferMaxSize,
+ std::size_t messageBufferMaxNumberEntries);
~ControlThreadOutputBuffer();
- static ControlThreadOutputBuffer* GetRtLoggingInstance()
+ static ControlThreadOutputBuffer*
+ GetRtLoggingInstance()
{
return RtLoggingInstance;
}
@@ -226,163 +241,207 @@ namespace armarx
bool updateReadBuffer() const;
//logging read
- void resetLoggingPosition() const;
+ void resetLoggingPosition() const;
void foreachNewLoggingEntry(ConsumerFunctor consumer);
void forLatestLoggingEntry(ConsumerFunctor consumer);
std::size_t getNumberOfBytes() const;
- template<class LoggingEntryT, class...Ts>
- RtMessageLogEntryBase* addMessageToLog(Ts&& ...args);
+ template <class LoggingEntryT, class... Ts>
+ RtMessageLogEntryBase* addMessageToLog(Ts&&... args);
private:
- friend class RobotUnitModule::Logging; ///TODO change code to make this unnecessary
+ friend class RobotUnitModule::Logging; ///TODO change code to make this unnecessary
/// @brief this pointer is used for rt message logging and is not null in the control thread
static thread_local ControlThreadOutputBuffer* RtLoggingInstance;
std::size_t toBounds(std::size_t idx) const;
//settings and initialization:
- bool isInitialized {false};
- std::size_t numEntries {0};
+ bool isInitialized{false};
+ std::size_t numEntries{0};
- std::atomic_size_t writePosition {0};
- mutable std::atomic_size_t onePastLoggingReadPosition {0};
- mutable std::atomic_size_t onePastReadPosition {0};
+ std::atomic_size_t writePosition{0};
+ mutable std::atomic_size_t onePastLoggingReadPosition{0};
+ mutable std::atomic_size_t onePastReadPosition{0};
std::vector<Entry> entries;
std::vector<std::uint8_t> data;
- std::size_t messageBufferSize {0};
- std::size_t messageBufferEntries {0};
+ std::size_t messageBufferSize{0};
+ std::size_t messageBufferEntries{0};
};
- using SensorAndControl = ControlThreadOutputBuffer::Entry;
-}
-
-#define ARMARX_RT_LOGF(...) (*(_detail_ARMARX_RT_LOGF(__FILE__, ARMARX_FUNCTION,__LINE__, __VA_ARGS__, true)))
-
-#define _detail_ARMARX_RT_LOGF(file_, func_, line_, FormatString, ...) \
- ([&]{ \
- using namespace ::armarx; \
- using RtMessageLogEntryBase = ControlThreadOutputBuffer::RtMessageLogEntryBase; \
- struct RtMessageLogEntry : RtMessageLogEntryBase \
- { \
- using TupleT = decltype(std::make_tuple(__VA_ARGS__)); \
- const TupleT tuple; \
- ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER \
- std::size_t line() const final override {return line_;} \
- std::string file() const final override {return file_;} \
- std::string func() const final override {return func_;} \
- std::string format() const final override \
- { \
- return TupleToStringF<0,std::tuple_size<TupleT>::value -1>(FormatString, tuple); \
- } \
- RtMessageLogEntry(TupleT tuple) : tuple{std::move(tuple)} {} \
- RtMessageLogEntry(const RtMessageLogEntry&) = default; \
- }; \
- if (::armarx::ControlThreadOutputBuffer::GetRtLoggingInstance()) { \
- return ::armarx::ControlThreadOutputBuffer::GetRtLoggingInstance() \
- ->addMessageToLog<RtMessageLogEntry>(__VA_ARGS__); \
- } else { \
- *(loghelper(file_, line_, func_)) << "Redirected RT Logging:\n" \
- << RtMessageLogEntry(std::make_tuple(__VA_ARGS__)).format(); \
- return dynamic_cast<RtMessageLogEntryBase*> \
- (&::armarx::detail::RtMessageLogEntryNull::Instance); \
- } \
- }())
-
-#define ARMARX_RT_LOGF_DEBUG(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::DEBUG)
-#define ARMARX_RT_LOGF_VERBOSE(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::VERBOSE)
-#define ARMARX_RT_LOGF_INFO(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::INFO)
-#define ARMARX_RT_LOGF_IMPORTANT(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::IMPORTANT)
-#define ARMARX_RT_LOGF_WARNING(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::WARN)
-#define ARMARX_RT_LOGF_WARN(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::WARN)
-#define ARMARX_RT_LOGF_ERROR(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::ERROR)
-#define ARMARX_RT_LOGF_FATAL(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::FATAL)
+ using SensorAndControl = ControlThreadOutputBuffer::Entry;
+} // namespace armarx
+
+#define ARMARX_RT_LOGF(...) \
+ (*(_detail_ARMARX_RT_LOGF(__FILE__, ARMARX_FUNCTION, __LINE__, __VA_ARGS__, true)))
+
+#define _detail_ARMARX_RT_LOGF(file_, func_, line_, FormatString, ...) \
+ ( \
+ [&] \
+ { \
+ using namespace ::armarx; \
+ using RtMessageLogEntryBase = ControlThreadOutputBuffer::RtMessageLogEntryBase; \
+ struct RtMessageLogEntry : RtMessageLogEntryBase \
+ { \
+ using TupleT = decltype(std::make_tuple(__VA_ARGS__)); \
+ const TupleT tuple; \
+ ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER \
+ std::size_t \
+ line() const final override \
+ { \
+ return line_; \
+ } \
+ std::string \
+ file() const final override \
+ { \
+ return file_; \
+ } \
+ std::string \
+ func() const final override \
+ { \
+ return func_; \
+ } \
+ std::string \
+ format() const final override \
+ { \
+ return TupleToStringF<0, std::tuple_size<TupleT>::value - 1>(FormatString, \
+ tuple); \
+ } \
+ RtMessageLogEntry(TupleT tuple) : tuple{std::move(tuple)} \
+ { \
+ } \
+ RtMessageLogEntry(const RtMessageLogEntry&) = default; \
+ }; \
+ if (::armarx::ControlThreadOutputBuffer::GetRtLoggingInstance()) \
+ { \
+ return ::armarx::ControlThreadOutputBuffer::GetRtLoggingInstance() \
+ ->addMessageToLog<RtMessageLogEntry>(__VA_ARGS__); \
+ } \
+ else \
+ { \
+ *(loghelper(file_, line_, func_)) \
+ << "Redirected RT Logging:\n" \
+ << RtMessageLogEntry(std::make_tuple(__VA_ARGS__)).format(); \
+ return dynamic_cast<RtMessageLogEntryBase*>( \
+ &::armarx::detail::RtMessageLogEntryNull::Instance); \
+ } \
+ }())
+
+#define ARMARX_RT_LOGF_DEBUG(...) \
+ ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::DEBUG)
+#define ARMARX_RT_LOGF_VERBOSE(...) \
+ ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::VERBOSE)
+#define ARMARX_RT_LOGF_INFO(...) \
+ ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::INFO)
+#define ARMARX_RT_LOGF_IMPORTANT(...) \
+ ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::IMPORTANT)
+#define ARMARX_RT_LOGF_WARNING(...) \
+ ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::WARN)
+#define ARMARX_RT_LOGF_WARN(...) \
+ ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::WARN)
+#define ARMARX_RT_LOGF_ERROR(...) \
+ ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::ERROR)
+#define ARMARX_RT_LOGF_FATAL(...) \
+ ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::FATAL)
//impl
namespace armarx::detail
{
- inline IceUtil::Time RtMessageLogEntryBase::getTime() const
+ inline IceUtil::Time
+ RtMessageLogEntryBase::getTime() const
{
return time;
}
- inline std::string RtMessageLogEntryDummy::format() const
+ inline std::string
+ RtMessageLogEntryDummy::format() const
{
- throw std::logic_error {"called 'format' of RtMessageLogEntryDummy"};
+ throw std::logic_error{"called 'format' of RtMessageLogEntryDummy"};
}
- inline std::size_t RtMessageLogEntryDummy::line() const
+ inline std::size_t
+ RtMessageLogEntryDummy::line() const
{
- throw std::logic_error {"called 'line' of RtMessageLogEntryDummy"};
+ throw std::logic_error{"called 'line' of RtMessageLogEntryDummy"};
}
- inline std::string RtMessageLogEntryDummy::file() const
+ inline std::string
+ RtMessageLogEntryDummy::file() const
{
- throw std::logic_error {"called 'file' of RtMessageLogEntryDummy"};
+ throw std::logic_error{"called 'file' of RtMessageLogEntryDummy"};
}
- inline std::string RtMessageLogEntryDummy::func() const
+ inline std::string
+ RtMessageLogEntryDummy::func() const
{
- throw std::logic_error {"called 'func' of RtMessageLogEntryDummy"};
+ throw std::logic_error{"called 'func' of RtMessageLogEntryDummy"};
}
- inline RtMessageLogBuffer::RtMessageLogBuffer(
- std::size_t bufferSize, std::size_t numEntries,
- std::size_t bufferMaxSize, std::size_t bufferMaxNumberEntries
- ):
- initialBufferSize {bufferSize},
- initialBufferEntryNumbers {numEntries},
- bufferMaxSize {bufferMaxSize},
- bufferMaxNumberEntries {bufferMaxNumberEntries},
+ inline RtMessageLogBuffer::RtMessageLogBuffer(std::size_t bufferSize,
+ std::size_t numEntries,
+ std::size_t bufferMaxSize,
+ std::size_t bufferMaxNumberEntries) :
+ initialBufferSize{bufferSize},
+ initialBufferEntryNumbers{numEntries},
+ bufferMaxSize{bufferMaxSize},
+ bufferMaxNumberEntries{bufferMaxNumberEntries},
buffer(bufferSize, 0),
- bufferSpace {buffer.size()},
- bufferPlace {buffer.data()},
+ bufferSpace{buffer.size()},
+ bufferPlace{buffer.data()},
entries(numEntries, nullptr)
- {}
+ {
+ }
inline RtMessageLogBuffer::~RtMessageLogBuffer()
{
deleteAll();
}
- inline const std::vector<const RtMessageLogEntryBase*>& RtMessageLogBuffer::getEntries() const
+ inline const std::vector<const RtMessageLogEntryBase*>&
+ RtMessageLogBuffer::getEntries() const
{
return entries;
}
- inline std::size_t RtMessageLogBuffer::getMaximalBufferSize() const
+ inline std::size_t
+ RtMessageLogBuffer::getMaximalBufferSize() const
{
return bufferMaxSize;
}
- inline std::size_t RtMessageLogBuffer::getMaximalNumberOfBufferEntries() const
+ inline std::size_t
+ RtMessageLogBuffer::getMaximalNumberOfBufferEntries() const
{
return bufferMaxNumberEntries;
}
- inline std::size_t ControlThreadOutputBufferEntry::getDataBufferSize() const
+ inline std::size_t
+ ControlThreadOutputBufferEntry::getDataBufferSize() const
{
return buffer.size();
}
-}
+} // namespace armarx::detail
namespace armarx
{
- inline std::size_t ControlThreadOutputBuffer::toBounds(std::size_t idx) const
+ inline std::size_t
+ ControlThreadOutputBuffer::toBounds(std::size_t idx) const
{
return idx % numEntries;
}
- inline std::size_t ControlThreadOutputBuffer::getNumberOfBytes() const
+ inline std::size_t
+ ControlThreadOutputBuffer::getNumberOfBytes() const
{
return data.size();
}
- template<class LoggingEntryT, class...Ts>
- inline ControlThreadOutputBuffer::RtMessageLogEntryBase* ControlThreadOutputBuffer::addMessageToLog(Ts&& ...args)
+ template <class LoggingEntryT, class... Ts>
+ inline ControlThreadOutputBuffer::RtMessageLogEntryBase*
+ ControlThreadOutputBuffer::addMessageToLog(Ts&&... args)
{
detail::RtMessageLogBuffer& messages = getWriteBuffer().messages;
if (messages.entries.size() <= messages.entriesWritten)
@@ -392,14 +451,19 @@ namespace armarx
return &detail::RtMessageLogEntryDummy::Instance;
}
messages.maxAlign = std::max(messages.maxAlign, alignof(LoggingEntryT));
- void* place = std::align(alignof(LoggingEntryT), sizeof(LoggingEntryT), messages.bufferPlace, messages.bufferSpace);
+ void* place = std::align(alignof(LoggingEntryT),
+ sizeof(LoggingEntryT),
+ messages.bufferPlace,
+ messages.bufferSpace);
if (!place)
{
- messages.requiredAdditionalBufferSpace += sizeof(LoggingEntryT) + alignof(LoggingEntryT) - 1;
+ messages.requiredAdditionalBufferSpace +=
+ sizeof(LoggingEntryT) + alignof(LoggingEntryT) - 1;
++messages.messagesLost;
return &detail::RtMessageLogEntryDummy::Instance;
}
- RtMessageLogEntryBase* entry = new (place) LoggingEntryT(std::make_tuple(std::forward<Ts>(args)...));
+ RtMessageLogEntryBase* entry =
+ new (place) LoggingEntryT(std::make_tuple(std::forward<Ts>(args)...));
ARMARX_CHECK_NOT_NULL(entry);
messages.bufferPlace = static_cast<std::uint8_t*>(place) + sizeof(LoggingEntryT);
@@ -409,4 +473,4 @@ namespace armarx
messages.entries.at(messages.entriesWritten++) = entry;
return entry;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/util/CtrlUtil.h b/source/RobotAPI/components/units/RobotUnit/util/CtrlUtil.h
index 706f9396e80e8680dfe75e935b4789c06bf806b7..02f14407354dc70fa2b72e57531943529f5af03b 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/CtrlUtil.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/CtrlUtil.h
@@ -23,26 +23,32 @@
*/
//pragma once
#include <cmath>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
#include <iostream>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
+
namespace armarx::ctrlutil
{
- double s(double t, double s0, double v0, double a0, double j)
+ double
+ s(double t, double s0, double v0, double a0, double j)
{
return s0 + v0 * t + 0.5 * a0 * t * t + 1.0 / 6.0 * j * t * t * t;
}
- double v(double t, double v0, double a0, double j)
+
+ double
+ v(double t, double v0, double a0, double j)
{
return v0 + a0 * t + 0.5 * j * t * t;
}
- double a(double t, double a0, double j)
+
+ double
+ a(double t, double a0, double j)
{
return a0 + j * t;
}
-
- double t_to_v(double v, double a, double j)
+ double
+ t_to_v(double v, double a, double j)
{
// time to accelerate to v with jerk j starting at acceleration a0
//return 2*math.sqrt(a0**2+2*j*(v/2))-a0/j
@@ -51,12 +57,11 @@ namespace armarx::ctrlutil
// --> p = 2a0/j; q = -2v/j
// t = - a0/j +- sqrt((a0/j)**2 +2v/j)
double tmp = a / j;
- return - a / j + std::sqrt(tmp * tmp + 2 * v / j);
+ return -a / j + std::sqrt(tmp * tmp + 2 * v / j);
}
-
-
- double t_to_v_with_wedge_acc(double v, double a0, double j)
+ double
+ t_to_v_with_wedge_acc(double v, double a0, double j)
{
// ramp up and down of acceleration (i.e. a0=at); returns t to achieve delta v
return 2 * t_to_v(v / 2.0, a0, j);
@@ -67,16 +72,25 @@ namespace armarx::ctrlutil
double s_total, v1, v2, v3, dt1, dt2, dt3;
};
- double brakingDistance(double v0, double a0, double j)
+ double
+ brakingDistance(double v0, double a0, double j)
{
auto signV = math::MathUtils::Sign(v0);
auto t = t_to_v(v0, -signV * a0, signV * j);
return s(t, 0, v0, a0, -signV * j);
}
- BrakingData brakingDistance(double goal, double s0, double v0, double a0, double v_max, double a_max, double j, double dec_max)
+ BrakingData
+ brakingDistance(double goal,
+ double s0,
+ double v0,
+ double a0,
+ double v_max,
+ double a_max,
+ double j,
+ double dec_max)
{
- double d = math::MathUtils::Sign(goal - s0); // if v0 == 0.0 else abs(v0)/a_max
+ double d = math::MathUtils::Sign(goal - s0); // if v0 == 0.0 else abs(v0)/a_max
dec_max *= -d;
// std::cout << "dec max: " << (dec_max) << " d: " << d << std::endl;
double dt1 = std::abs((dec_max - a0) / (-j * d));
@@ -84,7 +98,8 @@ namespace armarx::ctrlutil
// double a1 = a(dt1, a0, -d * j);
double v1 = v(dt1, v0, a0, -d * j);
double acc_duration = std::abs(dec_max) / j;
- double v2 = v(acc_duration, 0, 0, d * j);// # inverse of time to accelerate from 0 to max v
+ double v2 =
+ v(acc_duration, 0, 0, d * j); // # inverse of time to accelerate from 0 to max v
v2 = math::MathUtils::LimitTo(v2, v_max);
// v2 = v1 + dv2
double dt2 = d * ((v1 - v2) / dec_max);
@@ -97,7 +112,7 @@ namespace armarx::ctrlutil
double v3 = v(dt3, v2, dec_max, d * j);
double s_total = s1 + s2 + s3;
- if (dt2 < 0)// # we dont have a phase with a=a_max and need to reduce the a_max
+ if (dt2 < 0) // # we dont have a phase with a=a_max and need to reduce the a_max
{
double excess_time = t_to_v_with_wedge_acc(std::abs(v1), std::abs(dec_max), j);
double delta_a = a(excess_time / 2, 0, d * j);
@@ -115,28 +130,31 @@ namespace armarx::ctrlutil
{
double s_total, s1, s2, v1, v2, a1, a2, t, dt1, dt2;
};
- bool isPossibleToBrake(double v0, double a0, double j)
+
+ bool
+ isPossibleToBrake(double v0, double a0, double j)
{
return j * v0 - a0 * a0 / 2 > 0.0f;
}
- double jerk(double t, double s0, double v0, double a0)
+ double
+ jerk(double t, double s0, double v0, double a0)
{
return (6 * s0 - 3 * t * (a0 * t + 2 * v0)) / (t * t * t);
}
- std::tuple<double, double, double> calcAccAndJerk(double s0, double v0)
+ std::tuple<double, double, double>
+ calcAccAndJerk(double s0, double v0)
{
s0 *= -1;
- double t = - (3 * s0) / v0;
- double a0 = - (2 * v0) / t;
+ double t = -(3 * s0) / v0;
+ double a0 = -(2 * v0) / t;
double j = 2 * v0 / (t * t);
return std::make_tuple(t, a0, j);
}
-
-
- WedgeBrakingData braking_distance_with_wedge(double v0, double a0, double j)
+ WedgeBrakingData
+ braking_distance_with_wedge(double v0, double a0, double j)
{
// # v0 = v1 + v2
// # v1 = t1 * a0 + 0.5*j*t1**2
@@ -159,8 +177,7 @@ namespace armarx::ctrlutil
r.dt2 = -1;
return r;
throw std::runtime_error("unsuitable parameters! : j: " + std::to_string(j) +
- " a0: " + std::to_string(a0) +
- " v0: " + std::to_string(v0));
+ " a0: " + std::to_string(a0) + " v0: " + std::to_string(v0));
}
double t1 = std::sqrt(part) / j;
double t2 = (a0 / j) + t1;
@@ -180,4 +197,4 @@ namespace armarx::ctrlutil
double s_total = s1 + s2;
return {s_total, s1, s2, d * v1, d * v2, d * a1, d * a2, t1 + t2, t1, t2};
}
-}
+} // namespace armarx::ctrlutil
diff --git a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp
index bf5e5e28970861a2ebd6e0fb8c17d052962f5786..79d076f9b2aa7202f4ad649f76090b034da2f6db 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp
@@ -23,22 +23,25 @@
*/
#include "DynamicsHelper.h"
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/observers/filters/rtfilters/RTFilterBase.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+
namespace armarx
{
- DynamicsHelper::DynamicsHelper(RobotUnit* robotUnit): robotUnit(robotUnit)
+ DynamicsHelper::DynamicsHelper(RobotUnit* robotUnit) : robotUnit(robotUnit)
{
ARMARX_CHECK_EXPRESSION(robotUnit);
}
- void DynamicsHelper::addRobotPart(VirtualRobot::RobotNodeSetPtr rnsJoints, VirtualRobot::RobotNodeSetPtr rnsBodies,
- rtfilters::RTFilterBasePtr velocityFilter,
- rtfilters::RTFilterBasePtr accelerationFilter)
+ void
+ DynamicsHelper::addRobotPart(VirtualRobot::RobotNodeSetPtr rnsJoints,
+ VirtualRobot::RobotNodeSetPtr rnsBodies,
+ rtfilters::RTFilterBasePtr velocityFilter,
+ rtfilters::RTFilterBasePtr accelerationFilter)
{
DynamicsData data(rnsJoints, rnsBodies);
for (size_t i = 0; i < rnsJoints->getSize(); i++)
@@ -46,9 +49,11 @@ namespace armarx
auto selectedJoint = robotUnit->getSensorDevice(rnsJoints->getNode(i)->getName());
if (selectedJoint)
{
- auto sensorValue = const_cast<SensorValue1DoFActuator*>(selectedJoint->getSensorValue()->asA<SensorValue1DoFActuator>());
+ auto sensorValue = const_cast<SensorValue1DoFActuator*>(
+ selectedJoint->getSensorValue()->asA<SensorValue1DoFActuator>());
ARMARX_CHECK_EXPRESSION(sensorValue) << rnsJoints->getNode(i)->getName();
- ARMARX_DEBUG << "will calculate inverse dynamics for robot node " << rnsJoints->getNode(i)->getName();
+ ARMARX_DEBUG << "will calculate inverse dynamics for robot node "
+ << rnsJoints->getNode(i)->getName();
data.nodeSensorVec.push_back(std::make_pair(rnsJoints->getNode(i), sensorValue));
}
else
@@ -68,7 +73,9 @@ namespace armarx
dataMap.emplace(std::make_pair(rnsJoints, data));
}
- void DynamicsHelper::update(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ DynamicsHelper::update(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
for (auto& pair : dataMap)
{
@@ -81,17 +88,21 @@ namespace armarx
{
ARMARX_CHECK_EXPRESSION(node.second) << node.first->getName();
data.q(i) = node.second->position;
- if (i < data.velocityFilter.size() && data.velocityFilter.at(i) && !std::isnan(node.second->velocity))
+ if (i < data.velocityFilter.size() && data.velocityFilter.at(i) &&
+ !std::isnan(node.second->velocity))
{
- data.qDot(i) = data.velocityFilter.at(i)->update(sensorValuesTimestamp, node.second->velocity);
+ data.qDot(i) = data.velocityFilter.at(i)->update(sensorValuesTimestamp,
+ node.second->velocity);
}
else
{
data.qDot(i) = node.second->velocity;
}
- if (i < data.accelerationFilter.size() && data.accelerationFilter.at(i) && !std::isnan(node.second->acceleration))
+ if (i < data.accelerationFilter.size() && data.accelerationFilter.at(i) &&
+ !std::isnan(node.second->acceleration))
{
- data.qDDot(i) = data.accelerationFilter.at(i)->update(sensorValuesTimestamp, node.second->acceleration);
+ data.qDDot(i) = data.accelerationFilter.at(i)->update(
+ sensorValuesTimestamp, node.second->acceleration);
}
else
{
@@ -102,10 +113,7 @@ namespace armarx
}
// calculate external torques (tau) applied to robot induced by dynamics
dynamics.getGravityMatrix(data.q, data.tauGravity);
- dynamics.getInverseDynamics(data.q,
- data.qDot,
- data.qDDot,
- data.tau);
+ dynamics.getInverseDynamics(data.q, data.qDot, data.qDDot, data.tau);
// update virtual sensor values
size_t i = 0;
for (auto& node : data.nodeSensorVec)
diff --git a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h
index de313e9cee23c43e52a54617cbbbf29b2168ef72..edcf445b56cc0354889cce4475147aabf1605c2b 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h
@@ -22,49 +22,58 @@
* GNU General Public License
*/
#pragma once
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/Dynamics/Dynamics.h>
#include <Eigen/Core>
+
+#include <VirtualRobot/Dynamics/Dynamics.h>
+#include <VirtualRobot/RobotNodeSet.h>
+
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
namespace armarx::rtfilters
{
class RTFilterBase;
using RTFilterBasePtr = std::shared_ptr<RTFilterBase>;
-}
+} // namespace armarx::rtfilters
namespace armarx
{
class RobotUnit;
+
class DynamicsHelper
{
public:
struct DynamicsData
{
- DynamicsData(VirtualRobot::RobotNodeSetPtr rnsJoints, VirtualRobot::RobotNodeSetPtr rnsBodies) :
- rnsJoints(rnsJoints), rnsBodies(rnsBodies),
- dynamics(rnsJoints, rnsBodies)
+ DynamicsData(VirtualRobot::RobotNodeSetPtr rnsJoints,
+ VirtualRobot::RobotNodeSetPtr rnsBodies) :
+ rnsJoints(rnsJoints), rnsBodies(rnsBodies), dynamics(rnsJoints, rnsBodies)
{
q = qDot = qDDot = tau = tauGravity = Eigen::VectorXd::Zero(rnsJoints->getSize());
}
+
DynamicsData(const DynamicsData& r) = default;
VirtualRobot::RobotNodeSetPtr rnsJoints, rnsBodies;
VirtualRobot::Dynamics dynamics;
Eigen::VectorXd q, qDot, qDDot, tau, tauGravity;
- std::vector<std::pair<VirtualRobot::RobotNodePtr, SensorValue1DoFActuator*>> nodeSensorVec;
+ std::vector<std::pair<VirtualRobot::RobotNodePtr, SensorValue1DoFActuator*>>
+ nodeSensorVec;
std::vector<rtfilters::RTFilterBasePtr> velocityFilter;
std::vector<rtfilters::RTFilterBasePtr> accelerationFilter;
};
DynamicsHelper(RobotUnit* robotUnit);
~DynamicsHelper() = default;
- void addRobotPart(VirtualRobot::RobotNodeSetPtr rnsJoints, VirtualRobot::RobotNodeSetPtr rnsBodies,
- rtfilters::RTFilterBasePtr velocityFilter = rtfilters::RTFilterBasePtr(),
- rtfilters::RTFilterBasePtr accelerationFilter = rtfilters::RTFilterBasePtr());
- void update(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ void
+ addRobotPart(VirtualRobot::RobotNodeSetPtr rnsJoints,
+ VirtualRobot::RobotNodeSetPtr rnsBodies,
+ rtfilters::RTFilterBasePtr velocityFilter = rtfilters::RTFilterBasePtr(),
+ rtfilters::RTFilterBasePtr accelerationFilter = rtfilters::RTFilterBasePtr());
+ void update(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration);
+
protected:
RobotUnit* robotUnit;
std::map<VirtualRobot::RobotNodeSetPtr, DynamicsData> dataMap;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/util/EigenForwardDeclarations.h b/source/RobotAPI/components/units/RobotUnit/util/EigenForwardDeclarations.h
index 490f247702f1daa761c9b61c9f8959ae9e5dbadd..3d589313c9f40cd8b369db450006f7f25369e9b2 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/EigenForwardDeclarations.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/EigenForwardDeclarations.h
@@ -23,41 +23,41 @@
namespace Eigen
{
- template<typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+ template <typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
class Matrix;
-#define ax_eigen_fwd_make_matrix_and_vector(Type, TSuff, Size, SzSuff) \
- typedef Matrix<Type, Size, Size, 0, Size, Size> Matrix##SzSuff##TSuff; \
- typedef Matrix<Type, Size, 1 , 0, Size, 1 > Vector##SzSuff##TSuff;
+#define ax_eigen_fwd_make_matrix_and_vector(Type, TSuff, Size, SzSuff) \
+ typedef Matrix<Type, Size, Size, 0, Size, Size> Matrix##SzSuff##TSuff; \
+ typedef Matrix<Type, Size, 1, 0, Size, 1> Vector##SzSuff##TSuff;
-#define ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, Size) \
- typedef Matrix<Type, Size, -1,0, Size, -1> Matrix##Size##X##TypeSuffix; \
- typedef Matrix<Type, -1, Size,0, -1, Size> Matrix##X##Size##TypeSuffix;
+#define ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, Size) \
+ typedef Matrix<Type, Size, -1, 0, Size, -1> Matrix##Size##X##TypeSuffix; \
+ typedef Matrix<Type, -1, Size, 0, -1, Size> Matrix##X##Size##TypeSuffix;
-#define ax_eigen_fwd_make_matrix_and_vector_ALL_SIZES(Type, TypeSuffix) \
- ax_eigen_fwd_make_matrix_and_vector(Type, TypeSuffix, 2, 2) \
- ax_eigen_fwd_make_matrix_and_vector(Type, TypeSuffix, 3, 3) \
- ax_eigen_fwd_make_matrix_and_vector(Type, TypeSuffix, 4, 4) \
- ax_eigen_fwd_make_matrix_and_vector(Type, TypeSuffix, 5, 5) \
- ax_eigen_fwd_make_matrix_and_vector(Type, TypeSuffix, 6, 6) \
- ax_eigen_fwd_make_matrix_and_vector(Type, TypeSuffix, -1, X) \
- ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, 2) \
- ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, 3) \
- ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, 4) \
- ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, 5) \
- ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, 6)
+#define ax_eigen_fwd_make_matrix_and_vector_ALL_SIZES(Type, TypeSuffix) \
+ ax_eigen_fwd_make_matrix_and_vector( \
+ Type, TypeSuffix, 2, 2) ax_eigen_fwd_make_matrix_and_vector(Type, TypeSuffix, 3, 3) \
+ ax_eigen_fwd_make_matrix_and_vector( \
+ Type, TypeSuffix, 4, 4) ax_eigen_fwd_make_matrix_and_vector(Type, TypeSuffix, 5, 5) \
+ ax_eigen_fwd_make_matrix_and_vector(Type, TypeSuffix, 6, 6) \
+ ax_eigen_fwd_make_matrix_and_vector(Type, TypeSuffix, -1, X) \
+ ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, 2) \
+ ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, 3) \
+ ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, 4) \
+ ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, 5) \
+ ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, 6)
- ax_eigen_fwd_make_matrix_and_vector_ALL_SIZES(int, i)
- ax_eigen_fwd_make_matrix_and_vector_ALL_SIZES(float, f)
- ax_eigen_fwd_make_matrix_and_vector_ALL_SIZES(double, d)
+ ax_eigen_fwd_make_matrix_and_vector_ALL_SIZES(int, i)
+ ax_eigen_fwd_make_matrix_and_vector_ALL_SIZES(float, f)
+ ax_eigen_fwd_make_matrix_and_vector_ALL_SIZES(double, d)
#undef ax_eigen_fwd_make_matrix_and_vector_ALL_SIZES
#undef ax_eigen_fwd_make_matrix_and_vector
#undef ax_eigen_fwd_make_matrix_one_dynamic_dim
- template<typename _Scalar, int _Options>
- class Quaternion;
+ template <typename _Scalar, int _Options>
+ class Quaternion;
using Quaternionf = Quaternion<float, 0>;
using Quaterniond = Quaternion<double, 0>;
-}
+} // namespace Eigen
diff --git a/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainer.h b/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainer.h
index 8539b52e860fc21b785fad49e6d1a0c342c92ae1..a7d0ed6bc06bde7a1d48242070fbb76448636a68 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainer.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainer.h
@@ -25,15 +25,16 @@
#include <vector>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/util/TemplateMetaProgramming.h>
#include <ArmarXCore/core/util/PropagateConst.h>
+#include <ArmarXCore/core/util/TemplateMetaProgramming.h>
#include "HeterogenousContinuousContainerMacros.h"
-#if __GNUC__< 5 && !defined(__clang__)
+#if __GNUC__ < 5 && !defined(__clang__)
namespace std
{
- inline void* align(size_t alignment, size_t bytes, void*& bufferPlace, size_t& bufferSpace) noexcept
+ inline void*
+ align(size_t alignment, size_t bytes, void*& bufferPlace, size_t& bufferSpace) noexcept
{
const auto uiptr = reinterpret_cast<uintptr_t>(bufferPlace);
const auto alignedPlace = (uiptr - 1u + alignment) & -alignment;
@@ -48,39 +49,50 @@ namespace std
return bufferPlace = reinterpret_cast<void*>(alignedPlace);
}
}
-}
+} // namespace std
#endif
namespace armarx::detail
{
- template<class Base>
+ template <class Base>
class HeterogenousContinuousContainerBase
{
public:
- bool empty() const
+ bool
+ empty() const
{
return begin_ == current_;
}
- bool owning() const
+
+ bool
+ owning() const
{
return storage_ != nullptr;
}
- std::size_t getRemainingStorageCapacity() const
+
+ std::size_t
+ getRemainingStorageCapacity() const
{
return static_cast<const std::uint8_t*>(end_) -
static_cast<const std::uint8_t*>(current_);
}
- std::size_t getStorageCapacity() const
+
+ std::size_t
+ getStorageCapacity() const
{
return static_cast<const std::uint8_t*>(end_) -
static_cast<const std::uint8_t*>(begin_);
}
- std::size_t getUsedStorageCapacity() const
+
+ std::size_t
+ getUsedStorageCapacity() const
{
return static_cast<const std::uint8_t*>(current_) -
static_cast<const std::uint8_t*>(begin_);
}
- void assignStorage(void* begin, void* end)
+
+ void
+ assignStorage(void* begin, void* end)
{
ARMARX_CHECK_EXPRESSION(empty());
ARMARX_CHECK_LESS_EQUAL(begin, end);
@@ -89,7 +101,9 @@ namespace armarx::detail
current_ = begin;
end_ = end;
}
- void setStorageCapacity(std::size_t sz)
+
+ void
+ setStorageCapacity(std::size_t sz)
{
ARMARX_CHECK_EXPRESSION(empty());
if (!sz)
@@ -104,14 +118,15 @@ namespace armarx::detail
current_ = begin_;
end_ = static_cast<void*>(storage_.get() + sz);
}
+
protected:
- template<class Derived>
- Base* pushBack(const Derived* d)
+ template <class Derived>
+ Base*
+ pushBack(const Derived* d)
{
static_assert(
std::is_base_of<Base, Derived>::value,
- "HeterogenousContinuousContainerBase::pushBack: Derived must derive Base"
- );
+ "HeterogenousContinuousContainerBase::pushBack: Derived must derive Base");
std::size_t space_ = getRemainingStorageCapacity();
void* ptr = std::align(d->_alignof(), d->_sizeof(), current_, space_);
@@ -123,37 +138,42 @@ namespace armarx::detail
current_ = static_cast<void*>(static_cast<std::uint8_t*>(ptr) + d->_sizeof());
return d->_placementCopyConstruct(ptr);
}
- void clear()
+
+ void
+ clear()
{
current_ = begin_;
}
+
private:
- std::unique_ptr<std::uint8_t[]> storage_ {nullptr};
- void* begin_ {nullptr};
- void* current_ {nullptr};
- void* end_ {nullptr};
+ std::unique_ptr<std::uint8_t[]> storage_{nullptr};
+ void* begin_{nullptr};
+ void* current_{nullptr};
+ void* end_{nullptr};
};
-}
+} // namespace armarx::detail
namespace armarx
{
- template<class Base, bool UsePropagateConst = true>
+ template <class Base, bool UsePropagateConst = true>
class HeterogenousContinuousContainer : public detail::HeterogenousContinuousContainerBase<Base>
{
using BaseContainer = detail::HeterogenousContinuousContainerBase<Base>;
+
public:
- using ElementType = typename std::conditional <
- UsePropagateConst,
- PropagateConst<Base*>,
- Base *
- >::type;
+ using ElementType =
+ typename std::conditional<UsePropagateConst, PropagateConst<Base*>, Base*>::type;
HeterogenousContinuousContainer() = default;
HeterogenousContinuousContainer(HeterogenousContinuousContainer&&) = default;
- HeterogenousContinuousContainer(const HeterogenousContinuousContainer& other, bool compressElems = false)
+
+ HeterogenousContinuousContainer(const HeterogenousContinuousContainer& other,
+ bool compressElems = false)
{
- this->setStorageCapacity(compressElems ? other.getUsedStorageCapacity() : other.getStorageCapacity());
- setElementCapacity(compressElems ? other.elements().size() : other.elements().capacity());
+ this->setStorageCapacity(compressElems ? other.getUsedStorageCapacity()
+ : other.getStorageCapacity());
+ setElementCapacity(compressElems ? other.elements().size()
+ : other.elements().capacity());
for (auto& e : other.elements())
{
pushBack(e);
@@ -161,7 +181,9 @@ namespace armarx
}
HeterogenousContinuousContainer& operator=(HeterogenousContinuousContainer&&) = default;
- HeterogenousContinuousContainer& operator=(const HeterogenousContinuousContainer& other)
+
+ HeterogenousContinuousContainer&
+ operator=(const HeterogenousContinuousContainer& other)
{
clear();
setStorageCapacity(other.getStorageCapacity());
@@ -178,37 +200,43 @@ namespace armarx
clear();
}
- std::size_t getElementCount() const
+ std::size_t
+ getElementCount() const
{
return elements_.size();
}
- std::size_t getElementCapacity() const
+
+ std::size_t
+ getElementCapacity() const
{
return elements_.capacity();
}
- std::size_t getRemainingElementCapacity() const
+
+ std::size_t
+ getRemainingElementCapacity() const
{
return getElementCapacity() - getElementCount();
}
- void setElementCapacity(std::size_t cnt)
+ void
+ setElementCapacity(std::size_t cnt)
{
ARMARX_CHECK_EXPRESSION(this->empty());
if (elements_.capacity() > cnt)
{
//force the capacity to reduce
- elements_ = std::vector<ElementType> {};
+ elements_ = std::vector<ElementType>{};
}
elements_.reserve(cnt);
}
- template<class Derived>
- Base* pushBack(const Derived* d)
+ template <class Derived>
+ Base*
+ pushBack(const Derived* d)
{
static_assert(
std::is_base_of<Base, Derived>::value,
- "HeterogenousContinuousContainerBase::pushBack: Derived must derive Base"
- );
+ "HeterogenousContinuousContainerBase::pushBack: Derived must derive Base");
ARMARX_CHECK_NOT_NULL(d);
if (!getRemainingElementCapacity())
{
@@ -221,27 +249,35 @@ namespace armarx
}
return ptr;
}
- template<class Derived>
- Base* pushBack(const PropagateConst<Derived*>& d)
+
+ template <class Derived>
+ Base*
+ pushBack(const PropagateConst<Derived*>& d)
{
return pushBack(*d);
}
- template<class Derived>
- Base* pushBack(const Derived& d)
+
+ template <class Derived>
+ Base*
+ pushBack(const Derived& d)
{
return pushBack(&d);
}
- std::vector<ElementType>& elements()
+ std::vector<ElementType>&
+ elements()
{
return elements_;
}
- const std::vector<ElementType>& elements() const
+
+ const std::vector<ElementType>&
+ elements() const
{
return elements_;
}
- void clear()
+ void
+ clear()
{
for (auto& e : elements_)
{
@@ -250,26 +286,29 @@ namespace armarx
elements_.clear();
BaseContainer::clear();
}
+
private:
std::vector<ElementType> elements_;
};
- template<class Base, bool UsePropagateConst = true>
- class HeterogenousContinuous2DContainer : public detail::HeterogenousContinuousContainerBase<Base>
+ template <class Base, bool UsePropagateConst = true>
+ class HeterogenousContinuous2DContainer :
+ public detail::HeterogenousContinuousContainerBase<Base>
{
using BaseContainer = detail::HeterogenousContinuousContainerBase<Base>;
+
public:
- using ElementType = typename std::conditional <
- UsePropagateConst,
- PropagateConst<Base*>,
- Base *
- >::type;
+ using ElementType =
+ typename std::conditional<UsePropagateConst, PropagateConst<Base*>, Base*>::type;
HeterogenousContinuous2DContainer() = default;
HeterogenousContinuous2DContainer(HeterogenousContinuous2DContainer&&) = default;
- HeterogenousContinuous2DContainer(const HeterogenousContinuous2DContainer& other, bool compressElems = false)
+
+ HeterogenousContinuous2DContainer(const HeterogenousContinuous2DContainer& other,
+ bool compressElems = false)
{
- setStorageCapacity(compressElems ? other.getUsedStorageCapacity() : other.getStorageCapacity());
+ setStorageCapacity(compressElems ? other.getUsedStorageCapacity()
+ : other.getStorageCapacity());
std::vector<std::size_t> elemCaps;
elemCaps.reserve(other.elements().size());
for (const auto& d1 : other.elements())
@@ -287,7 +326,9 @@ namespace armarx
}
HeterogenousContinuous2DContainer& operator=(HeterogenousContinuous2DContainer&&) = default;
- HeterogenousContinuous2DContainer& operator=(const HeterogenousContinuous2DContainer& other)
+
+ HeterogenousContinuous2DContainer&
+ operator=(const HeterogenousContinuous2DContainer& other)
{
clear();
setStorageCapacity(other.getStorageCapacity());
@@ -313,20 +354,26 @@ namespace armarx
clear();
}
- std::size_t getElementCount(std::size_t d0) const
+ std::size_t
+ getElementCount(std::size_t d0) const
{
return elements_.at(d0).size();
}
- std::size_t getElementCapacity(std::size_t d0) const
+
+ std::size_t
+ getElementCapacity(std::size_t d0) const
{
return elements_.at(d0).capacity();
}
- std::size_t getRemainingElementCapacity(std::size_t d0) const
+
+ std::size_t
+ getRemainingElementCapacity(std::size_t d0) const
{
return getElementCapacity(d0) - getElementCount(d0);
}
- void setElementCapacity(const std::vector<std::size_t>& cnt)
+ void
+ setElementCapacity(const std::vector<std::size_t>& cnt)
{
ARMARX_CHECK_EXPRESSION(this->empty());
elements_.resize(cnt.size());
@@ -335,19 +382,19 @@ namespace armarx
if (elements_.at(i).capacity() > cnt)
{
//force the capacity to reduce
- elements_.at(i) = std::vector<ElementType> {};
+ elements_.at(i) = std::vector<ElementType>{};
}
elements_.at(i).reserve(cnt.at(i));
}
}
- template<class Derived>
- Base* pushBack(std::size_t d0, const Derived* d)
+ template <class Derived>
+ Base*
+ pushBack(std::size_t d0, const Derived* d)
{
static_assert(
std::is_base_of<Base, Derived>::value,
- "HeterogenousContinuousContainerBase::pushBack: Derived must derive Base"
- );
+ "HeterogenousContinuousContainerBase::pushBack: Derived must derive Base");
ARMARX_CHECK_NOT_NULL(d);
if (!getRemainingElementCapacity(d0))
{
@@ -360,27 +407,35 @@ namespace armarx
}
return ptr;
}
- template<class Derived>
- Base* pushBack(std::size_t d0, const PropagateConst<Derived*>& d)
+
+ template <class Derived>
+ Base*
+ pushBack(std::size_t d0, const PropagateConst<Derived*>& d)
{
return pushBack(d0, *d);
}
- template<class Derived>
- Base* pushBack(std::size_t d0, const Derived& d)
+
+ template <class Derived>
+ Base*
+ pushBack(std::size_t d0, const Derived& d)
{
return pushBack(d0, &d);
}
- std::vector<std::vector<ElementType>>& elements()
+ std::vector<std::vector<ElementType>>&
+ elements()
{
return elements_;
}
- const std::vector<std::vector<ElementType>>& elements() const
+
+ const std::vector<std::vector<ElementType>>&
+ elements() const
{
return elements_;
}
- void clear()
+ void
+ clear()
{
for (auto& d1 : elements_)
{
@@ -392,7 +447,8 @@ namespace armarx
}
BaseContainer::clear();
}
+
private:
std::vector<std::vector<ElementType>> elements_;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainerMacros.h b/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainerMacros.h
index 07415ee68c3bb1b1f97d3667b90981ab64448351..5ee3b4e087522de3615600cbccf0f895ee2334bb 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainerMacros.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainerMacros.h
@@ -25,64 +25,61 @@
#include <memory>
#include <type_traits>
-#define ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER_BASE(CommonBaseType) \
- using _typeBase = CommonBaseType; \
- virtual std::size_t _alignof() const = 0; \
- virtual std::size_t _sizeof() const = 0; \
- virtual _typeBase* _placementCopyConstruct(void* place) const = 0; \
- std::size_t _accumulateSize(std::size_t offset = 0) const \
- { \
- return offset + \
- (_alignof() - (offset % _alignof())) % _alignof() + \
- _sizeof(); \
+#define ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER_BASE(CommonBaseType) \
+ using _typeBase = CommonBaseType; \
+ virtual std::size_t _alignof() const = 0; \
+ virtual std::size_t _sizeof() const = 0; \
+ virtual _typeBase* _placementCopyConstruct(void* place) const = 0; \
+ std::size_t _accumulateSize(std::size_t offset = 0) const \
+ { \
+ return offset + (_alignof() - (offset % _alignof())) % _alignof() + _sizeof(); \
}
-#define ARMARX_PLACEMENT_CONSTRUCTION_HELPER_BASE(CommonBaseType) \
- ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER_BASE(CommonBaseType) \
- virtual void _copyTo(_typeBase* target) const = 0; \
- virtual std::unique_ptr<_typeBase> _clone() const = 0; \
+#define ARMARX_PLACEMENT_CONSTRUCTION_HELPER_BASE(CommonBaseType) \
+ ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER_BASE(CommonBaseType) \
+ virtual void _copyTo(_typeBase* target) const = 0; \
+ virtual std::unique_ptr<_typeBase> _clone() const = 0; \
virtual _typeBase* _placementConstruct(void* place) const = 0;
-#define ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER \
- std::size_t _alignof() const override \
- { \
- using _type = typename std::decay<decltype(*this)>::type; \
- return alignof(_type); \
- } \
- std::size_t _sizeof() const override \
- { \
- using _type = typename std::decay<decltype(*this)>::type; \
- return sizeof(_type); \
- } \
- _typeBase* _placementCopyConstruct(void* place) const override \
- { \
- using _type = typename std::decay<decltype(*this)>::type; \
- return new(place) _type(*this); \
- } \
- void _checkBaseType() \
- { \
- using _type = typename std::decay<decltype(*this)>::type; \
- static_assert( \
- std::is_base_of<_typeBase, _type>::value, \
- "This class has to derive the common base class"); \
+#define ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER \
+ std::size_t _alignof() const override \
+ { \
+ using _type = typename std::decay<decltype(*this)>::type; \
+ return alignof(_type); \
+ } \
+ std::size_t _sizeof() const override \
+ { \
+ using _type = typename std::decay<decltype(*this)>::type; \
+ return sizeof(_type); \
+ } \
+ _typeBase* _placementCopyConstruct(void* place) const override \
+ { \
+ using _type = typename std::decay<decltype(*this)>::type; \
+ return new (place) _type(*this); \
+ } \
+ void _checkBaseType() \
+ { \
+ using _type = typename std::decay<decltype(*this)>::type; \
+ static_assert(std::is_base_of<_typeBase, _type>::value, \
+ "This class has to derive the common base class"); \
}
-#define ARMARX_PLACEMENT_CONSTRUCTION_HELPER \
- ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER \
- void _copyTo(_typeBase* target) const override \
- { \
- using _type = typename std::decay<decltype(*this)>::type; \
- _type* const castedTarget = dynamic_cast<_type*>(target); \
- ARMARX_CHECK_NOT_NULL(castedTarget); \
- *castedTarget = *this; \
- } \
- std::unique_ptr<_typeBase> _clone() const override \
- { \
- using _type = typename std::decay<decltype(*this)>::type; \
- return std::unique_ptr<_typeBase>{new _type(*this)}; \
- } \
- _typeBase* _placementConstruct(void* place) const override \
- { \
- using _type = typename std::decay<decltype(*this)>::type; \
- return new(place) _type; \
+#define ARMARX_PLACEMENT_CONSTRUCTION_HELPER \
+ ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER \
+ void _copyTo(_typeBase* target) const override \
+ { \
+ using _type = typename std::decay<decltype(*this)>::type; \
+ _type* const castedTarget = dynamic_cast<_type*>(target); \
+ ARMARX_CHECK_NOT_NULL(castedTarget); \
+ *castedTarget = *this; \
+ } \
+ std::unique_ptr<_typeBase> _clone() const override \
+ { \
+ using _type = typename std::decay<decltype(*this)>::type; \
+ return std::unique_ptr<_typeBase>{new _type(*this)}; \
+ } \
+ _typeBase* _placementConstruct(void* place) const override \
+ { \
+ using _type = typename std::decay<decltype(*this)>::type; \
+ return new (place) _type; \
}
diff --git a/source/RobotAPI/components/units/RobotUnit/util/JointAndNJointControllers.h b/source/RobotAPI/components/units/RobotUnit/util/JointAndNJointControllers.h
index 84809506f96ca0f0d48211fe2b7511f75b08d8f9..c3c645936a9cab1e9d61baee4ef2640e6ba4e0f4 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/JointAndNJointControllers.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/JointAndNJointControllers.h
@@ -27,16 +27,19 @@ namespace armarx
{
class JointController;
class NJointControllerBase;
+
/// @brief Structure used by the RobotUnit to swap lists of Joint and NJoint controllers
struct JointAndNJointControllers
{
- JointAndNJointControllers(std::size_t n = 0)
- : jointControllers(n),
- nJointControllers(n),
- jointToNJointControllerAssignement(n, Sentinel())
- {}
+ JointAndNJointControllers(std::size_t n = 0) :
+ jointControllers(n),
+ nJointControllers(n),
+ jointToNJointControllerAssignement(n, Sentinel())
+ {
+ }
- void resetAssignement()
+ void
+ resetAssignement()
{
jointToNJointControllerAssignement.assign(jointControllers.size(), Sentinel());
}
@@ -46,9 +49,10 @@ namespace armarx
/// @brief this is set by RobotUnit::writeRequestedControllers
std::vector<std::size_t> jointToNJointControllerAssignement;
- static constexpr std::size_t Sentinel()
+ static constexpr std::size_t
+ Sentinel()
{
return std::numeric_limits<std::size_t>::max();
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/util/KeyValueVector.h b/source/RobotAPI/components/units/RobotUnit/util/KeyValueVector.h
index be82c776f26e326f76642dda211d662a2b9e13f9..01dce758fa86860eeaf1524d58433450179acaf6 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/KeyValueVector.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/KeyValueVector.h
@@ -21,4 +21,3 @@
*/
#pragma once
#include <ArmarXCore/util/CPPUtility/KeyValueVector.h>
-
diff --git a/source/RobotAPI/components/units/RobotUnit/util/RtTiming.h b/source/RobotAPI/components/units/RobotUnit/util/RtTiming.h
index a9feb50c732672846a87813a87c9a309bc3c1aad..4e88ef1f3a8fae9e6baeeb453aa4711a40d77f30 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/RtTiming.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/RtTiming.h
@@ -29,27 +29,32 @@
namespace armarx
{
- inline IceUtil::Time rtNow()
+ inline IceUtil::Time
+ rtNow()
{
return IceUtil::Time::now(IceUtil::Time::Monotonic);
// using namespace std::chrono;
// auto epoch = time_point_cast<microseconds>(high_resolution_clock::now()).time_since_epoch();
// return IceUtil::Time::microSeconds(duration_cast<milliseconds>(epoch).count());
}
-}
+} // namespace armarx
#define RT_TIMING_START(name) auto name = armarx::rtNow();
//! \ingroup VirtualTime
//! Prints duration with comment in front of it, yet only once per second.
-#define RT_TIMING_END_COMMENT(name, comment) ARMARX_RT_LOGF_INFO("%s - duration: %.3f ms", comment, IceUtil::Time(armarx::rtNow()-name).toMilliSecondsDouble()).deactivateSpam(1);
+#define RT_TIMING_END_COMMENT(name, comment) \
+ ARMARX_RT_LOGF_INFO("%s - duration: %.3f ms", \
+ comment, \
+ IceUtil::Time(armarx::rtNow() - name).toMilliSecondsDouble()) \
+ .deactivateSpam(1);
//! \ingroup VirtualTime
//! Prints duration
-#define RT_TIMING_END(name) RT_TIMING_END_COMMENT(name,#name)
+#define RT_TIMING_END(name) RT_TIMING_END_COMMENT(name, #name)
//! \ingroup VirtualTime
//! Prints duration with comment in front of it if it took longer than threshold
-#define RT_TIMING_CEND_COMMENT(name, comment, thresholdMs) if((armarx::rtNow()-name).toMilliSecondsDouble() >= thresholdMs) RT_TIMING_END_COMMENT(name, comment)
+#define RT_TIMING_CEND_COMMENT(name, comment, thresholdMs) \
+ if ((armarx::rtNow() - name).toMilliSecondsDouble() >= thresholdMs) \
+ RT_TIMING_END_COMMENT(name, comment)
//! \ingroup VirtualTime
//! Prints duration if it took longer than thresholdMs
#define RT_TIMING_CEND(name, thresholdMs) RT_TIMING_CEND_COMMENT(name, #name, thresholdMs)
-
-
diff --git a/source/RobotAPI/components/units/RobotUnit/util/Time.h b/source/RobotAPI/components/units/RobotUnit/util/Time.h
index 9199a9714d09951ef6ae4df3f9b0f82f335a9845..f4aa7903585bade190632881c41d2894c9b5a33d 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/Time.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/Time.h
@@ -23,12 +23,13 @@
#pragma once
#include <chrono>
+
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
namespace armarx::detail
{
- template<class TimeT = std::chrono::microseconds>
+ template <class TimeT = std::chrono::microseconds>
struct TimerTag
{
//assume it is std::chrono
@@ -36,15 +37,18 @@ namespace armarx::detail
using ClockT = std::chrono::high_resolution_clock;
const ClockT::time_point beg;
- TimerTag() : beg {ClockT::now()} {}
+ TimerTag() : beg{ClockT::now()}
+ {
+ }
- TimeT stop()
+ TimeT
+ stop()
{
return std::chrono::duration_cast<TimeT>(ClockT::now() - beg);
}
};
- template<>
+ template <>
struct TimerTag<long>
{
//return micro seconds as long
@@ -52,34 +56,41 @@ namespace armarx::detail
using ClockT = std::chrono::high_resolution_clock;
const ClockT::time_point beg;
- TimerTag() : beg {ClockT::now()} {}
+ TimerTag() : beg{ClockT::now()}
+ {
+ }
- long stop()
+ long
+ stop()
{
- return std::chrono::duration_cast<std::chrono::microseconds>(ClockT::now() - beg).count();
+ return std::chrono::duration_cast<std::chrono::microseconds>(ClockT::now() - beg)
+ .count();
}
};
- template<>
+ template <>
struct TimerTag<TimestampVariant> : TimerTag<>
{
- TimestampVariant stop()
+ TimestampVariant
+ stop()
{
return {TimerTag<std::chrono::microseconds>::stop().count()};
}
};
- template<>
+ template <>
struct TimerTag<IceUtil::Time> : TimerTag<>
{
- TimestampVariant stop()
+ TimestampVariant
+ stop()
{
return IceUtil::Time::microSeconds(TimerTag<std::chrono::microseconds>::stop().count());
}
};
template <class Fun, class TimeT>
- TimeT operator*(TimerTag<TimeT>&& t, Fun&& fn)
+ TimeT
+ operator*(TimerTag<TimeT>&& t, Fun&& fn)
{
fn();
return t.stop();
@@ -87,38 +98,49 @@ namespace armarx::detail
//for virtual time
- template<class TimeT = IceUtil::Time>
+ template <class TimeT = IceUtil::Time>
struct VirtualTimerTag;
- template<>
+ template <>
struct VirtualTimerTag<TimestampVariant>
{
const IceUtil::Time beg;
- VirtualTimerTag() : beg {TimeUtil::GetTime()} {}
- TimestampVariant stop()
+
+ VirtualTimerTag() : beg{TimeUtil::GetTime()}
+ {
+ }
+
+ TimestampVariant
+ stop()
{
return {TimeUtil::GetTime() - beg};
}
};
- template<>
+ template <>
struct VirtualTimerTag<IceUtil::Time>
{
const IceUtil::Time beg;
- VirtualTimerTag() : beg {TimeUtil::GetTime()} {}
- TimestampVariant stop()
+
+ VirtualTimerTag() : beg{TimeUtil::GetTime()}
+ {
+ }
+
+ TimestampVariant
+ stop()
{
return TimeUtil::GetTime() - beg;
}
};
template <class Fun, class TimeT>
- TimeT operator*(VirtualTimerTag<TimeT>&& t, Fun&& fn)
+ TimeT
+ operator*(VirtualTimerTag<TimeT>&& t, Fun&& fn)
{
fn();
return t.stop();
}
-}
+} // namespace armarx::detail
-#define ARMARX_STOPWATCH(...) ::armarx::detail::TimerTag<__VA_ARGS__>{} *[&]
-#define ARMARX_VIRTUAL_STOPWATCH(...) ::armarx::detail::VirtualTimerTag<__VA_ARGS__>{} *[&]
+#define ARMARX_STOPWATCH(...) ::armarx::detail::TimerTag<__VA_ARGS__>{}* [&]
+#define ARMARX_VIRTUAL_STOPWATCH(...) ::armarx::detail::VirtualTimerTag<__VA_ARGS__>{}* [&]
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
index ccb1d28518ea3e30c3cb6cde8347cd1c3fdf8978..b247f12167424f45a2e87c690461d5aaab5516d0 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
@@ -21,34 +21,36 @@
*/
#pragma once
-#include "ClassMemberInfoEntry.h"
-
#include <ArmarXCore/util/CPPUtility/KeyValueVector.h>
+#include "ClassMemberInfoEntry.h"
namespace armarx::introspection
{
/**
* @brief
*/
- template<class CommonBaseT, class ClassT>
+ template <class CommonBaseT, class ClassT>
struct ClassMemberInfo
{
using ClassType = ClassT;
using CommonBaseType = CommonBaseT;
- static_assert(std::is_base_of<CommonBaseType, ClassType>::value, "The class has to inherit the common base class");
+ static_assert(std::is_base_of<CommonBaseType, ClassType>::value,
+ "The class has to inherit the common base class");
using Entry = introspection::ClassMemberInfoEntry<CommonBaseType>;
- template<class T>
- using EntryConfigurator = introspection::ClassMemberInfoEntryConfigurator<CommonBaseType, T>;
+ template <class T>
+ using EntryConfigurator =
+ introspection::ClassMemberInfoEntryConfigurator<CommonBaseType, T>;
static const ClassMemberInfo<CommonBaseT, ClassT>& GetInstance();
/// @brief add a member variable of the current class
- template<class MemberType>
- EntryConfigurator<ClassType> addMemberVariable(MemberType ClassType::* ptr, const std::string& name);
+ template <class MemberType>
+ EntryConfigurator<ClassType> addMemberVariable(MemberType ClassType::*ptr,
+ const std::string& name);
/// @brief add all variables of a base class of the current class
- template<class BaseClassType>
+ template <class BaseClassType>
void addBaseClass();
/// @brief Get the name of the current class
@@ -56,35 +58,40 @@ namespace armarx::introspection
/// @brief Get all entries for member variables
static const KeyValueVector<std::string, Entry>& GetEntries();
static std::size_t GetNumberOfDataFields();
- static void GetDataFieldAs(const ClassType* ptr, std::size_t i, bool& out);
- static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Byte& out);
- static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Short& out);
- static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Int& out);
- static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Long& out);
- static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Float& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, bool& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Byte& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Short& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Int& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Long& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Float& out);
static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Double& out);
static void GetDataFieldAs(const ClassType* ptr, std::size_t i, std::string& out);
static const std::type_info& GetDataFieldType(std::size_t i);
static std::vector<std::string> GetDataFieldNames();
- static std::map<std::string, VariantBasePtr> ToVariants(const IceUtil::Time& timestamp, const CommonBaseT* ptr);
+ static std::map<std::string, VariantBasePtr> ToVariants(const IceUtil::Time& timestamp,
+ const CommonBaseT* ptr);
+
private:
- template<class T>
- static auto MakeConverter()
+ template <class T>
+ static auto
+ MakeConverter()
{
ARMARX_TRACE;
std::vector<std::function<void(const ClassType*, T&)>> functions;
for (std::size_t idxEntr = 0; idxEntr < GetEntries().size(); ++idxEntr)
{
- for (std::size_t idxField = 0; idxField < GetEntries().at(idxEntr).getNumberOfFields(); ++idxField)
+ for (std::size_t idxField = 0;
+ idxField < GetEntries().at(idxEntr).getNumberOfFields();
+ ++idxField)
{
functions.emplace_back(
- [idxEntr, idxField](const ClassType * classptr, T & out)
- {
- const Entry& e = GetEntries().at(idxEntr);
- e.getDataFieldAs(idxField, classptr, out);
- });
+ [idxEntr, idxField](const ClassType* classptr, T& out)
+ {
+ const Entry& e = GetEntries().at(idxEntr);
+ e.getDataFieldAs(idxField, classptr, out);
+ });
}
}
ARMARX_CHECK_EQUAL(functions.size(), GetNumberOfDataFields());
@@ -95,15 +102,16 @@ namespace armarx::introspection
KeyValueVector<std::string, Entry> entries;
std::set<std::string> addedBases;
};
-}
-
+} // namespace armarx::introspection
//implementation
namespace armarx::introspection
{
- template<class CommonBaseT, class ClassT>
- template<class MemberType>
- typename ClassMemberInfo<CommonBaseT, ClassT>::template EntryConfigurator<ClassT> ClassMemberInfo<CommonBaseT, ClassT>::addMemberVariable(MemberType ClassType::* ptr, const std::string& name)
+ template <class CommonBaseT, class ClassT>
+ template <class MemberType>
+ typename ClassMemberInfo<CommonBaseT, ClassT>::template EntryConfigurator<ClassT>
+ ClassMemberInfo<CommonBaseT, ClassT>::addMemberVariable(MemberType ClassType::*ptr,
+ const std::string& name)
{
ARMARX_TRACE;
ARMARX_CHECK_EQUAL(0, entries.count(name));
@@ -111,18 +119,22 @@ namespace armarx::introspection
return entries.at(name);
}
- template<class CommonBaseT, class ClassT>
- template<class BaseClassType>
- void ClassMemberInfo<CommonBaseT, ClassT>::addBaseClass()
+ template <class CommonBaseT, class ClassT>
+ template <class BaseClassType>
+ void
+ ClassMemberInfo<CommonBaseT, ClassT>::addBaseClass()
{
ARMARX_TRACE;
if (std::is_same<BaseClassType, CommonBaseType>::value)
{
return;
}
- static_assert(std::is_base_of<CommonBaseType, BaseClassType>::value, "The base class has to inherit the common base class");
- static_assert(std::is_base_of<BaseClassType, ClassType>::value, "The base class has to be a base class");
- static_assert(!std::is_same<BaseClassType, ClassType>::value, "The base class has must not be the class");
+ static_assert(std::is_base_of<CommonBaseType, BaseClassType>::value,
+ "The base class has to inherit the common base class");
+ static_assert(std::is_base_of<BaseClassType, ClassType>::value,
+ "The base class has to be a base class");
+ static_assert(!std::is_same<BaseClassType, ClassType>::value,
+ "The base class has must not be the class");
std::set<std::string> basesAddedInCall;
if (addedBases.count(ClassMemberInfo<CommonBaseType, BaseClassType>::GetClassName()))
@@ -133,11 +145,11 @@ namespace armarx::introspection
{
if (!addedBases.count(e.getClassName()))
{
- ARMARX_CHECK_EXPRESSION(
- !entries.count(e.getMemberName())) <<
- "Adding the base class '" << GetTypeString<BaseClassType>()
- << "' adds an entry for the member '" << e.getMemberName()
- << "' which already was added by class '" << entries.at(e.getMemberName()).getClassName() << "'";
+ ARMARX_CHECK_EXPRESSION(!entries.count(e.getMemberName()))
+ << "Adding the base class '" << GetTypeString<BaseClassType>()
+ << "' adds an entry for the member '" << e.getMemberName()
+ << "' which already was added by class '"
+ << entries.at(e.getMemberName()).getClassName() << "'";
basesAddedInCall.emplace(e.getClassName());
entries.add(e.getMemberName(), e);
}
@@ -145,8 +157,9 @@ namespace armarx::introspection
addedBases.insert(basesAddedInCall.begin(), basesAddedInCall.end());
}
- template<class CommonBaseT, class ClassT>
- std::vector<std::string> ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldNames()
+ template <class CommonBaseT, class ClassT>
+ std::vector<std::string>
+ ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldNames()
{
ARMARX_TRACE;
std::vector<std::string> dataFieldNames;
@@ -162,8 +175,10 @@ namespace armarx::introspection
return dataFieldNames;
}
- template<class CommonBaseT, class ClassT>
- std::map<std::string, VariantBasePtr> ClassMemberInfo<CommonBaseT, ClassT>::ToVariants(const IceUtil::Time& timestamp, const CommonBaseT* ptr)
+ template <class CommonBaseT, class ClassT>
+ std::map<std::string, VariantBasePtr>
+ ClassMemberInfo<CommonBaseT, ClassT>::ToVariants(const IceUtil::Time& timestamp,
+ const CommonBaseT* ptr)
{
ARMARX_TRACE;
std::map<std::string, VariantBasePtr> result;
@@ -174,7 +189,8 @@ namespace armarx::introspection
{
if (result.count(elem.first))
{
- throw std::invalid_argument {"mergeMaps: newMap would override values from oldMap"};
+ throw std::invalid_argument{
+ "mergeMaps: newMap would override values from oldMap"};
}
}
for (auto& elem : newMap)
@@ -186,27 +202,31 @@ namespace armarx::introspection
return result;
}
- template<class CommonBaseT, class ClassT>
- const ClassMemberInfo<CommonBaseT, ClassT>& ClassMemberInfo<CommonBaseT, ClassT>::GetInstance()
+ template <class CommonBaseT, class ClassT>
+ const ClassMemberInfo<CommonBaseT, ClassT>&
+ ClassMemberInfo<CommonBaseT, ClassT>::GetInstance()
{
static const ClassMemberInfo<CommonBaseT, ClassT> i = ClassT::GetClassMemberInfo();
return i;
}
- template<class CommonBaseT, class ClassT>
- const std::string& ClassMemberInfo<CommonBaseT, ClassT>::GetClassName()
+ template <class CommonBaseT, class ClassT>
+ const std::string&
+ ClassMemberInfo<CommonBaseT, ClassT>::GetClassName()
{
return GetTypeString<ClassType>();
}
- template<class CommonBaseT, class ClassT>
- const KeyValueVector<std::string, ClassMemberInfoEntry<CommonBaseT>>& ClassMemberInfo<CommonBaseT, ClassT>::GetEntries()
+ template <class CommonBaseT, class ClassT>
+ const KeyValueVector<std::string, ClassMemberInfoEntry<CommonBaseT>>&
+ ClassMemberInfo<CommonBaseT, ClassT>::GetEntries()
{
return GetInstance().entries;
}
- template<class CommonBaseT, class ClassT>
- std::size_t ClassMemberInfo<CommonBaseT, ClassT>::GetNumberOfDataFields()
+ template <class CommonBaseT, class ClassT>
+ std::size_t
+ ClassMemberInfo<CommonBaseT, ClassT>::GetNumberOfDataFields()
{
ARMARX_TRACE;
static const std::size_t numberOfDataFields = []
@@ -221,64 +241,95 @@ namespace armarx::introspection
return numberOfDataFields;
}
- template<class CommonBaseT, class ClassT>
- void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, bool& out)
+ template <class CommonBaseT, class ClassT>
+ void
+ ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr,
+ std::size_t i,
+ bool& out)
{
ARMARX_TRACE;
static const auto convert = MakeConverter<bool>();
ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
convert.at(i)(ptr, out);
}
- template<class CommonBaseT, class ClassT>
- void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Byte& out)
+
+ template <class CommonBaseT, class ClassT>
+ void
+ ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr,
+ std::size_t i,
+ Ice::Byte& out)
{
ARMARX_TRACE;
static const auto convert = MakeConverter<Ice::Byte>();
ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
convert.at(i)(ptr, out);
}
- template<class CommonBaseT, class ClassT>
- void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Short& out)
+
+ template <class CommonBaseT, class ClassT>
+ void
+ ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr,
+ std::size_t i,
+ Ice::Short& out)
{
ARMARX_TRACE;
static const auto convert = MakeConverter<Ice::Short>();
ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
convert.at(i)(ptr, out);
}
- template<class CommonBaseT, class ClassT>
- void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Int& out)
+
+ template <class CommonBaseT, class ClassT>
+ void
+ ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr,
+ std::size_t i,
+ Ice::Int& out)
{
ARMARX_TRACE;
static const auto convert = MakeConverter<Ice::Int>();
ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
convert.at(i)(ptr, out);
}
- template<class CommonBaseT, class ClassT>
- void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Long& out)
+
+ template <class CommonBaseT, class ClassT>
+ void
+ ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr,
+ std::size_t i,
+ Ice::Long& out)
{
ARMARX_TRACE;
static const auto convert = MakeConverter<Ice::Long>();
ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
convert.at(i)(ptr, out);
}
- template<class CommonBaseT, class ClassT>
- void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Float& out)
+
+ template <class CommonBaseT, class ClassT>
+ void
+ ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr,
+ std::size_t i,
+ Ice::Float& out)
{
ARMARX_TRACE;
static const auto convert = MakeConverter<Ice::Float>();
ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
convert.at(i)(ptr, out);
}
- template<class CommonBaseT, class ClassT>
- void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Double& out)
+
+ template <class CommonBaseT, class ClassT>
+ void
+ ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr,
+ std::size_t i,
+ Ice::Double& out)
{
ARMARX_TRACE;
static const auto convert = MakeConverter<Ice::Double>();
ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
convert.at(i)(ptr, out);
}
- template<class CommonBaseT, class ClassT>
- void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, std::string& out)
+
+ template <class CommonBaseT, class ClassT>
+ void
+ ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr,
+ std::size_t i,
+ std::string& out)
{
ARMARX_TRACE;
static const auto convert = MakeConverter<std::string>();
@@ -286,8 +337,9 @@ namespace armarx::introspection
convert.at(i)(ptr, out);
}
- template<class CommonBaseT, class ClassT>
- const std::type_info& ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldType(std::size_t i)
+ template <class CommonBaseT, class ClassT>
+ const std::type_info&
+ ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldType(std::size_t i)
{
ARMARX_TRACE;
static const auto convert = []
@@ -297,7 +349,9 @@ namespace armarx::introspection
for (std::size_t idxEntr = 0; idxEntr < GetEntries().size(); ++idxEntr)
{
const Entry& e = GetEntries().at(idxEntr);
- for (std::size_t idxField = 0; idxField < GetEntries().at(idxEntr).getNumberOfFields(); ++idxField)
+ for (std::size_t idxField = 0;
+ idxField < GetEntries().at(idxEntr).getNumberOfFields();
+ ++idxField)
{
data.emplace_back(&e.getFieldTypeId(idxField));
}
@@ -308,4 +362,4 @@ namespace armarx::introspection
ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
return *convert.at(i);
}
-}
+} // namespace armarx::introspection
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
index 1ad242babb61a5f2970b641d0ac3bbb09c338ba1..4fe0b74a29f29bbeeba89fe0593144b6c34dac51 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
@@ -26,12 +26,12 @@
namespace armarx::introspection
{
- template<class CommonBaseT, class ClassT>
+ template <class CommonBaseT, class ClassT>
struct ClassMemberInfo;
- template<class CommonBaseT, class ClassT>
+ template <class CommonBaseT, class ClassT>
struct ClassMemberInfoEntryConfigurator;
- template<class CommonBaseT>
+ template <class CommonBaseT>
struct ClassMemberInfoEntry
{
public:
@@ -42,14 +42,14 @@ namespace armarx::introspection
ClassMemberInfoEntry& operator=(ClassMemberInfoEntry&&) = default;
ClassMemberInfoEntry& operator=(const ClassMemberInfoEntry&) = default;
- template<class ClassType, class MemberType>
- ClassMemberInfoEntry(const std::string& memberName, MemberType ClassType::* ptr):
- className {GetTypeString<ClassType>()},
- memberName {memberName},
- memberTypeName {GetTypeString<MemberType>()},
- numberOfFields {DataFieldsInfo<MemberType>::GetNumberOfFields()},
- fieldToType {MakeFieldToTypeFunctors<ClassType>(numberOfFields, ptr)},
- toVariants_ {MakeToVariantsFunctor<ClassType>(ptr)}
+ template <class ClassType, class MemberType>
+ ClassMemberInfoEntry(const std::string& memberName, MemberType ClassType::*ptr) :
+ className{GetTypeString<ClassType>()},
+ memberName{memberName},
+ memberTypeName{GetTypeString<MemberType>()},
+ numberOfFields{DataFieldsInfo<MemberType>::GetNumberOfFields()},
+ fieldToType{MakeFieldToTypeFunctors<ClassType>(numberOfFields, ptr)},
+ toVariants_{MakeToVariantsFunctor<ClassType>(ptr)}
{
ARMARX_CHECK_NOT_EQUAL(0, numberOfFields);
//field names
@@ -70,113 +70,140 @@ namespace armarx::introspection
}
//general
- const std::string& getClassName() const
+ const std::string&
+ getClassName() const
{
return className;
}
- const std::string& getMemberName() const
+
+ const std::string&
+ getMemberName() const
{
return memberName;
}
- const std::string& getMemberTypeName() const
+
+ const std::string&
+ getMemberTypeName() const
{
return memberTypeName;
}
+
//fields
- std::size_t getNumberOfFields() const
+ std::size_t
+ getNumberOfFields() const
{
return numberOfFields;
}
- const std::string& getFieldName(std::size_t i) const
+
+ const std::string&
+ getFieldName(std::size_t i) const
{
ARMARX_CHECK_LESS(i, numberOfFields);
return fieldNames.at(i);
}
- void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, bool& out) const
+
+ void
+ getDataFieldAs(std::size_t i, const CommonBaseType* ptr, bool& out) const
{
ARMARX_CHECK_NOT_NULL(ptr);
ARMARX_CHECK_LESS(i, numberOfFields);
return fieldToType.at(i).toBool(ptr, out);
}
- void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Byte& out) const
+
+ void
+ getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Byte& out) const
{
ARMARX_CHECK_NOT_NULL(ptr);
ARMARX_CHECK_LESS(i, numberOfFields);
return fieldToType.at(i).toByte(ptr, out);
}
- void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Short& out) const
+
+ void
+ getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Short& out) const
{
ARMARX_CHECK_NOT_NULL(ptr);
ARMARX_CHECK_LESS(i, numberOfFields);
return fieldToType.at(i).toShort(ptr, out);
}
- void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Int& out) const
+
+ void
+ getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Int& out) const
{
ARMARX_CHECK_NOT_NULL(ptr);
ARMARX_CHECK_LESS(i, numberOfFields);
return fieldToType.at(i).toInt(ptr, out);
}
- void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Long& out) const
+
+ void
+ getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Long& out) const
{
ARMARX_CHECK_NOT_NULL(ptr);
ARMARX_CHECK_LESS(i, numberOfFields);
return fieldToType.at(i).toLong(ptr, out);
}
- void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Float& out) const
+
+ void
+ getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Float& out) const
{
ARMARX_CHECK_NOT_NULL(ptr);
ARMARX_CHECK_LESS(i, numberOfFields);
return fieldToType.at(i).toFloat(ptr, out);
}
- void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Double& out) const
+
+ void
+ getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Double& out) const
{
ARMARX_CHECK_NOT_NULL(ptr);
ARMARX_CHECK_LESS(i, numberOfFields);
return fieldToType.at(i).toDouble(ptr, out);
}
- void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, std::string& out) const
+
+ void
+ getDataFieldAs(std::size_t i, const CommonBaseType* ptr, std::string& out) const
{
ARMARX_CHECK_NOT_NULL(ptr);
ARMARX_CHECK_LESS(i, numberOfFields);
return fieldToType.at(i).toString(ptr, out);
}
- const std::type_info& getFieldTypeId(std::size_t i) const
+
+ const std::type_info&
+ getFieldTypeId(std::size_t i) const
{
ARMARX_CHECK_LESS(i, numberOfFields);
return *fieldToType.at(i).typeId;
}
//variants
- std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp, const CommonBaseType* ptr) const
+ std::map<std::string, VariantBasePtr>
+ toVariants(const IceUtil::Time& timestamp, const CommonBaseType* ptr) const
{
return toVariants_(this, timestamp, ptr);
}
private:
- using ToVariantsFunctorType = std::function <
- std::map<std::string, VariantBasePtr>(
- const ClassMemberInfoEntry* thisptr,
- const IceUtil::Time&,
- const CommonBaseType*) >;
+ using ToVariantsFunctorType =
+ std::function<std::map<std::string, VariantBasePtr>(const ClassMemberInfoEntry* thisptr,
+ const IceUtil::Time&,
+ const CommonBaseType*)>;
+
struct FieldToType
{
- template<class T>
+ template <class T>
using FieldToFunctorType = std::function<void(const CommonBaseType*, T&)>;
- FieldToFunctorType<bool> toBool;
- FieldToFunctorType<Ice::Byte> toByte;
- FieldToFunctorType<Ice::Short> toShort;
- FieldToFunctorType<Ice::Int> toInt;
- FieldToFunctorType<Ice::Long> toLong;
- FieldToFunctorType<Ice::Float> toFloat;
+ FieldToFunctorType<bool> toBool;
+ FieldToFunctorType<Ice::Byte> toByte;
+ FieldToFunctorType<Ice::Short> toShort;
+ FieldToFunctorType<Ice::Int> toInt;
+ FieldToFunctorType<Ice::Long> toLong;
+ FieldToFunctorType<Ice::Float> toFloat;
FieldToFunctorType<Ice::Double> toDouble;
FieldToFunctorType<std::string> toString;
- const std::type_info* typeId;
+ const std::type_info* typeId;
};
- template<class ClassType, class MemberType, class MemberPtrClassType>
- static std::vector<FieldToType> MakeFieldToTypeFunctors(
- std::size_t numberOfFields,
- MemberType MemberPtrClassType::* ptr)
+ template <class ClassType, class MemberType, class MemberPtrClassType>
+ static std::vector<FieldToType>
+ MakeFieldToTypeFunctors(std::size_t numberOfFields, MemberType MemberPtrClassType::*ptr)
{
std::vector<FieldToType> result;
result.reserve(numberOfFields);
@@ -185,46 +212,48 @@ namespace armarx::introspection
result.emplace_back();
auto& fieldData = result.back();
-#define make_getter(Type) [i, ptr](const CommonBaseType * ptrBase, Type & out) \
- { \
- const ClassType* cptr = dynamic_cast<const ClassType*>(ptrBase); \
- ARMARX_CHECK_NOT_NULL(cptr); \
- DataFieldsInfo<MemberType>::GetDataFieldAs(i, cptr->*ptr, out); \
+#define make_getter(Type) \
+ [i, ptr](const CommonBaseType* ptrBase, Type& out) \
+ { \
+ const ClassType* cptr = dynamic_cast<const ClassType*>(ptrBase); \
+ ARMARX_CHECK_NOT_NULL(cptr); \
+ DataFieldsInfo<MemberType>::GetDataFieldAs(i, cptr->*ptr, out); \
}
- fieldData.toBool = make_getter(bool);
- fieldData.toByte = make_getter(Ice::Byte);
- fieldData.toShort = make_getter(Ice::Short);
- fieldData.toInt = make_getter(Ice::Int);
- fieldData.toLong = make_getter(Ice::Long);
- fieldData.toFloat = make_getter(Ice::Float);
+ fieldData.toBool = make_getter(bool);
+ fieldData.toByte = make_getter(Ice::Byte);
+ fieldData.toShort = make_getter(Ice::Short);
+ fieldData.toInt = make_getter(Ice::Int);
+ fieldData.toLong = make_getter(Ice::Long);
+ fieldData.toFloat = make_getter(Ice::Float);
fieldData.toDouble = make_getter(Ice::Double);
fieldData.toString = make_getter(std::string);
#undef make_getter
- fieldData.typeId = &DataFieldsInfo<MemberType>::GetDataFieldType(i);
+ fieldData.typeId = &DataFieldsInfo<MemberType>::GetDataFieldType(i);
}
return result;
}
- template<class ClassType, class MemberType, class MemberPtrClassType>
- static ToVariantsFunctorType MakeToVariantsFunctor(MemberType MemberPtrClassType::* ptr)
+ template <class ClassType, class MemberType, class MemberPtrClassType>
+ static ToVariantsFunctorType
+ MakeToVariantsFunctor(MemberType MemberPtrClassType::*ptr)
{
- return [ptr](
- const ClassMemberInfoEntry * thisptr,
- const IceUtil::Time & timestamp,
- const CommonBaseType * ptrBase)
+ return [ptr](const ClassMemberInfoEntry* thisptr,
+ const IceUtil::Time& timestamp,
+ const CommonBaseType* ptrBase)
{
const ClassType* cptr = dynamic_cast<const ClassType*>(ptrBase);
ARMARX_CHECK_NOT_NULL(cptr);
- return DataFieldsInfo<MemberType>::ToVariants(
- cptr->*ptr, thisptr->getMemberName(),
- timestamp,
- thisptr->variantReportFrame(), thisptr->variantReportAgent());
+ return DataFieldsInfo<MemberType>::ToVariants(cptr->*ptr,
+ thisptr->getMemberName(),
+ timestamp,
+ thisptr->variantReportFrame(),
+ thisptr->variantReportAgent());
};
}
- template<class BaseType, class ClassT>
+ template <class BaseType, class ClassT>
friend class ClassMemberInfo;
- template<class BaseType, class ClassT>
+ template <class BaseType, class ClassT>
friend class ClassMemberInfoEntryConfigurator;
const std::string className;
@@ -236,46 +265,56 @@ namespace armarx::introspection
std::vector<std::string> fieldNames;
//variants
ToVariantsFunctorType toVariants_;
- bool setVariantReportFrame_ {false};
- std::function<std::string()> variantReportFrame {[]{return "";}};
- std::function<std::string()> variantReportAgent {[]{return "";}};
+ bool setVariantReportFrame_{false};
+ std::function<std::string()> variantReportFrame{[] { return ""; }};
+ std::function<std::string()> variantReportAgent{[] { return ""; }};
};
- template<class CommonBaseT, class ClassT>
+ template <class CommonBaseT, class ClassT>
struct ClassMemberInfoEntryConfigurator
{
using ClassType = ClassT;
using CommonBaseType = CommonBaseT;
using Entry = ClassMemberInfoEntry<CommonBaseType>;
- ClassMemberInfoEntryConfigurator& setFieldNames(std::vector<std::string> fieldNames)
+ ClassMemberInfoEntryConfigurator&
+ setFieldNames(std::vector<std::string> fieldNames)
{
ARMARX_CHECK_EQUAL(fieldNames.size(), e.numberOfFields);
e.fieldNames = std::move(fieldNames);
return *this;
}
- ClassMemberInfoEntryConfigurator& setVariantReportFrame(const std::string& agent, const std::string& frame)
+
+ ClassMemberInfoEntryConfigurator&
+ setVariantReportFrame(const std::string& agent, const std::string& frame)
{
e.setVariantReportFrame_ = true;
- e.variantReportFrame = [frame] {return frame;};
- e.variantReportAgent = [agent] {return agent;};
+ e.variantReportFrame = [frame] { return frame; };
+ e.variantReportAgent = [agent] { return agent; };
return *this;
}
- ClassMemberInfoEntryConfigurator& setVariantReportFrame(const std::string& agent, std::function<std::string()> frame)
+
+ ClassMemberInfoEntryConfigurator&
+ setVariantReportFrame(const std::string& agent, std::function<std::string()> frame)
{
e.setVariantReportFrame_ = true;
e.variantReportFrame = std::move(frame);
- e.variantReportAgent = [agent] {return agent;};
+ e.variantReportAgent = [agent] { return agent; };
return *this;
}
- ClassMemberInfoEntryConfigurator& setVariantReportFrame(std::function<std::string()> agent, const std::string& frame)
+
+ ClassMemberInfoEntryConfigurator&
+ setVariantReportFrame(std::function<std::string()> agent, const std::string& frame)
{
e.setVariantReportFrame_ = true;
- e.variantReportFrame = [frame] {return frame;};
+ e.variantReportFrame = [frame] { return frame; };
e.variantReportAgent = std::move(agent);
return *this;
}
- ClassMemberInfoEntryConfigurator& setVariantReportFrame(std::function<std::string()> agent, std::function<std::string()> frame)
+
+ ClassMemberInfoEntryConfigurator&
+ setVariantReportFrame(std::function<std::string()> agent,
+ std::function<std::string()> frame)
{
e.setVariantReportFrame_ = true;
e.variantReportFrame = std::move(frame);
@@ -283,13 +322,14 @@ namespace armarx::introspection
return *this;
}
- ClassMemberInfoEntryConfigurator& setVariantReportFunction(
- std::function<std::map<std::string, VariantBasePtr>(const IceUtil::Time&, const ClassType*)> f)
+ ClassMemberInfoEntryConfigurator&
+ setVariantReportFunction(
+ std::function<std::map<std::string, VariantBasePtr>(const IceUtil::Time&,
+ const ClassType*)> f)
{
- e.toVariants_ = [f](
- const ClassMemberInfoEntry<CommonBaseType>* thisptr,
- const IceUtil::Time & timestamp,
- const CommonBaseType * ptrBase)
+ e.toVariants_ = [f](const ClassMemberInfoEntry<CommonBaseType>* thisptr,
+ const IceUtil::Time& timestamp,
+ const CommonBaseType* ptrBase)
{
const ClassType* ptr = dynamic_cast<const ClassType*>(ptrBase);
ARMARX_CHECK_NOT_NULL(ptr);
@@ -300,7 +340,11 @@ namespace armarx::introspection
private:
friend struct ClassMemberInfo<CommonBaseType, ClassType>;
- ClassMemberInfoEntryConfigurator(Entry& e): e(e) {}
+
+ ClassMemberInfoEntryConfigurator(Entry& e) : e(e)
+ {
+ }
+
Entry& e;
};
-}
+} // namespace armarx::introspection
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp
index 08bc46984505141a1631e669b3e8b229ea2940e2..2131c04fc9e5bd7a31fbddd78222c8d4d0ad4485 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp
@@ -26,45 +26,59 @@
#include <Eigen/Geometry>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/core/OrientedPoint.h>
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/OrientedPoint.h>
+#include <RobotAPI/libraries/core/Pose.h>
//Eigen::Vector3f
namespace armarx::introspection
{
- std::size_t DataFieldsInfo<Eigen::Vector3f, void>::GetNumberOfFields()
+ std::size_t
+ DataFieldsInfo<Eigen::Vector3f, void>::GetNumberOfFields()
{
return 3;
}
- void DataFieldsInfo<Eigen::Vector3f, void>::GetDataFieldAs(std::size_t i, const Eigen::Vector3f& field, std::string& out)
+
+ void
+ DataFieldsInfo<Eigen::Vector3f, void>::GetDataFieldAs(std::size_t i,
+ const Eigen::Vector3f& field,
+ std::string& out)
{
ARMARX_CHECK_LESS(i, 3);
out = to_string(field(i));
}
- void DataFieldsInfo<Eigen::Vector3f, void>::GetDataFieldAs(std::size_t i, const Eigen::Vector3f& field, float& out)
+
+ void
+ DataFieldsInfo<Eigen::Vector3f, void>::GetDataFieldAs(std::size_t i,
+ const Eigen::Vector3f& field,
+ float& out)
{
ARMARX_CHECK_LESS(i, 3);
out = field(i);
}
- const std::type_info& DataFieldsInfo<Eigen::Vector3f, void>::GetDataFieldType(std::size_t i)
+
+ const std::type_info&
+ DataFieldsInfo<Eigen::Vector3f, void>::GetDataFieldType(std::size_t i)
{
return typeid(float);
}
- const std::vector<std::string>& DataFieldsInfo<Eigen::Vector3f, void>::GetFieldNames()
+
+ const std::vector<std::string>&
+ DataFieldsInfo<Eigen::Vector3f, void>::GetFieldNames()
{
static const std::vector<std::string> result{"x", "y", "z"};
return result;
}
- std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Vector3f, void>::ToVariants(
- const Eigen::Vector3f& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent)
+
+ std::map<std::string, VariantBasePtr>
+ DataFieldsInfo<Eigen::Vector3f, void>::ToVariants(const Eigen::Vector3f& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent)
{
if (!frame.empty())
{
@@ -73,110 +87,143 @@ namespace armarx::introspection
ARMARX_CHECK_EXPRESSION(agent.empty()) << "No frame but an agent given";
return {{name, {new TimedVariant(Vector3{value}, timestamp)}}};
}
-}
+} // namespace armarx::introspection
+
//Eigen::Vector##Num##Type
namespace armarx::introspection
{
-#define make_DataFieldsInfo_for_eigen_vector(Type,TypeName,Num) \
- void DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, std::string& out) \
- { \
- ARMARX_CHECK_LESS(i, Num); \
- out = to_string(field(i)); \
- } \
- void DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, Ice::TypeName& out) \
- { \
- ARMARX_CHECK_LESS(i, Num); \
- out = field(i); \
- } \
- const std::type_info& DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetDataFieldType(std::size_t i) \
- { \
- return typeid(Ice::TypeName); \
- } \
- std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Vector##Num##Type, void>::ToVariants( \
- const Eigen::Vector##Num##Type& value, \
- const std::string& name, \
- const IceUtil::Time& timestamp, \
- const std::string& frame, \
- const std::string& agent) \
- { \
- ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) << "There is no framed version of TimestampVariant"; \
- std::map<std::string, VariantBasePtr> result; \
- for(int i = 0; i < Num; ++i) \
- { \
- result.emplace(name + "." + to_string(i), VariantBasePtr{new TimedVariant(value(i), timestamp)}); \
- } \
- return result; \
- } \
- const std::vector<std::string>& DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetFieldNames() \
- { \
- static const std::vector<std::string> result = [] \
- { \
- std::vector<std::string> r; \
- for(int i = 0; i < Num; ++i) \
- { \
- r.emplace_back(to_string(i)); \
- } \
- return r; \
- }(); \
- return result; \
- }
-
- make_DataFieldsInfo_for_eigen_vector(f, Float, 2)
- make_DataFieldsInfo_for_eigen_vector(f, Float, 4)
- make_DataFieldsInfo_for_eigen_vector(f, Float, 5)
- make_DataFieldsInfo_for_eigen_vector(f, Float, 6)
-
- make_DataFieldsInfo_for_eigen_vector(d, Double, 2)
- make_DataFieldsInfo_for_eigen_vector(d, Double, 3)
- make_DataFieldsInfo_for_eigen_vector(d, Double, 4)
- make_DataFieldsInfo_for_eigen_vector(d, Double, 5)
- make_DataFieldsInfo_for_eigen_vector(d, Double, 6)
-
- make_DataFieldsInfo_for_eigen_vector(i, Int, 2)
- make_DataFieldsInfo_for_eigen_vector(i, Int, 3)
- make_DataFieldsInfo_for_eigen_vector(i, Int, 4)
- make_DataFieldsInfo_for_eigen_vector(i, Int, 5)
- make_DataFieldsInfo_for_eigen_vector(i, Int, 6)
+#define make_DataFieldsInfo_for_eigen_vector(Type, TypeName, Num) \
+ void DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetDataFieldAs( \
+ std::size_t i, const Eigen::Vector##Num##Type& field, std::string& out) \
+ { \
+ ARMARX_CHECK_LESS(i, Num); \
+ out = to_string(field(i)); \
+ } \
+ void DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetDataFieldAs( \
+ std::size_t i, const Eigen::Vector##Num##Type& field, Ice::TypeName& out) \
+ { \
+ ARMARX_CHECK_LESS(i, Num); \
+ out = field(i); \
+ } \
+ const std::type_info& DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetDataFieldType( \
+ std::size_t i) \
+ { \
+ return typeid(Ice::TypeName); \
+ } \
+ std::map<std::string, VariantBasePtr> \
+ DataFieldsInfo<Eigen::Vector##Num##Type, void>::ToVariants( \
+ const Eigen::Vector##Num##Type& value, \
+ const std::string& name, \
+ const IceUtil::Time& timestamp, \
+ const std::string& frame, \
+ const std::string& agent) \
+ { \
+ ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) \
+ << "There is no framed version of TimestampVariant"; \
+ std::map<std::string, VariantBasePtr> result; \
+ for (int i = 0; i < Num; ++i) \
+ { \
+ result.emplace(name + "." + to_string(i), \
+ VariantBasePtr{new TimedVariant(value(i), timestamp)}); \
+ } \
+ return result; \
+ } \
+ const std::vector<std::string>& \
+ DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetFieldNames() \
+ { \
+ static const std::vector<std::string> result = [] \
+ { \
+ std::vector<std::string> r; \
+ for (int i = 0; i < Num; ++i) \
+ { \
+ r.emplace_back(to_string(i)); \
+ } \
+ return r; \
+ }(); \
+ return result; \
+ }
+
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 2) make_DataFieldsInfo_for_eigen_vector(f,
+ Float,
+ 4)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 5)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 6)
+
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 2)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 3)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 4)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 5)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 6)
+
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 2)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 3)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 4)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 5)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 6)
#undef make_DataFieldsInfo_for_eigen_vector
-}
+} // namespace armarx::introspection
+
//Eigen::Matrix4f
namespace armarx::introspection
{
- std::size_t DataFieldsInfo<Eigen::Matrix4f, void>::GetNumberOfFields()
+ std::size_t
+ DataFieldsInfo<Eigen::Matrix4f, void>::GetNumberOfFields()
{
return 16;
}
- void DataFieldsInfo<Eigen::Matrix4f, void>::GetDataFieldAs(std::size_t i, const Eigen::Matrix4f& field, float& out)
+
+ void
+ DataFieldsInfo<Eigen::Matrix4f, void>::GetDataFieldAs(std::size_t i,
+ const Eigen::Matrix4f& field,
+ float& out)
{
ARMARX_CHECK_LESS(i, 16);
out = field(i / 4, i % 4);
}
- void DataFieldsInfo<Eigen::Matrix4f, void>::GetDataFieldAs(std::size_t i, const Eigen::Matrix4f& field, std::string& out)
+
+ void
+ DataFieldsInfo<Eigen::Matrix4f, void>::GetDataFieldAs(std::size_t i,
+ const Eigen::Matrix4f& field,
+ std::string& out)
{
ARMARX_CHECK_LESS(i, 16);
out = to_string(field(i / 4, i % 4));
}
- const std::type_info& DataFieldsInfo<Eigen::Matrix4f, void>::GetDataFieldType(std::size_t i)
+
+ const std::type_info&
+ DataFieldsInfo<Eigen::Matrix4f, void>::GetDataFieldType(std::size_t i)
{
return typeid(float);
}
- const std::vector<std::string>& DataFieldsInfo<Eigen::Matrix4f, void>::GetFieldNames()
- {
- static const std::vector<std::string> result
- {
- "00", "01", "02", "03",
- "10", "11", "12", "13",
- "20", "21", "22", "23",
- "30", "31", "32", "33"
- };
+
+ const std::vector<std::string>&
+ DataFieldsInfo<Eigen::Matrix4f, void>::GetFieldNames()
+ {
+ static const std::vector<std::string> result{"00",
+ "01",
+ "02",
+ "03",
+ "10",
+ "11",
+ "12",
+ "13",
+ "20",
+ "21",
+ "22",
+ "23",
+ "30",
+ "31",
+ "32",
+ "33"};
return result;
}
- std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Matrix4f, void>::ToVariants(
- const Eigen::Matrix4f& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent)
+
+ std::map<std::string, VariantBasePtr>
+ DataFieldsInfo<Eigen::Matrix4f, void>::ToVariants(const Eigen::Matrix4f& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent)
{
if (!frame.empty())
{
@@ -185,15 +232,21 @@ namespace armarx::introspection
ARMARX_CHECK_EXPRESSION(agent.empty()) << "No frame but an agent given";
return {{name, {new TimedVariant(Pose{value}, timestamp)}}};
}
-}
+} // namespace armarx::introspection
+
//Eigen::Quaternionf
namespace armarx::introspection
{
- std::size_t DataFieldsInfo<Eigen::Quaternionf, void>::GetNumberOfFields()
+ std::size_t
+ DataFieldsInfo<Eigen::Quaternionf, void>::GetNumberOfFields()
{
return 4;
}
- void DataFieldsInfo<Eigen::Quaternionf, void>::GetDataFieldAs(std::size_t i, const Eigen::Quaternionf& field, float& out)
+
+ void
+ DataFieldsInfo<Eigen::Quaternionf, void>::GetDataFieldAs(std::size_t i,
+ const Eigen::Quaternionf& field,
+ float& out)
{
ARMARX_CHECK_LESS(i, 4);
switch (i)
@@ -211,14 +264,14 @@ namespace armarx::introspection
out = field.z();
return;
}
- throw std::logic_error
- {
- __FILE__ " : " + to_string(__LINE__) +
- " : Unreachable code reached"
- };
-
+ throw std::logic_error{__FILE__ " : " + to_string(__LINE__) +
+ " : Unreachable code reached"};
}
- void DataFieldsInfo<Eigen::Quaternionf, void>::GetDataFieldAs(std::size_t i, const Eigen::Quaternionf& field, std::string& out)
+
+ void
+ DataFieldsInfo<Eigen::Quaternionf, void>::GetDataFieldAs(std::size_t i,
+ const Eigen::Quaternionf& field,
+ std::string& out)
{
ARMARX_CHECK_LESS(i, 4);
switch (i)
@@ -236,30 +289,29 @@ namespace armarx::introspection
out = to_string(field.z());
return;
}
- throw std::logic_error
- {
- __FILE__ " : " + to_string(__LINE__) +
- " : Unreachable code reached"
- };
-
+ throw std::logic_error{__FILE__ " : " + to_string(__LINE__) +
+ " : Unreachable code reached"};
}
- const std::type_info& DataFieldsInfo<Eigen::Quaternionf, void>::GetDataFieldType(std::size_t i)
+ const std::type_info&
+ DataFieldsInfo<Eigen::Quaternionf, void>::GetDataFieldType(std::size_t i)
{
return typeid(float);
}
- const std::vector<std::string>& DataFieldsInfo<Eigen::Quaternionf, void>::GetFieldNames()
+
+ const std::vector<std::string>&
+ DataFieldsInfo<Eigen::Quaternionf, void>::GetFieldNames()
{
static const std::vector<std::string> result{"qw", "qx", "qy", "qz"};
return result;
}
- std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Quaternionf, void>::ToVariants(
- const Eigen::Quaternionf& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent)
+ std::map<std::string, VariantBasePtr>
+ DataFieldsInfo<Eigen::Quaternionf, void>::ToVariants(const Eigen::Quaternionf& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent)
{
if (!frame.empty())
{
@@ -268,81 +320,116 @@ namespace armarx::introspection
ARMARX_CHECK_EXPRESSION(agent.empty()) << "No frame but an agent given";
return {{name, {new TimedVariant(Quaternion{value}, timestamp)}}};
}
-}
+} // namespace armarx::introspection
+
//std::chrono::microseconds
namespace armarx::introspection
{
- std::size_t DataFieldsInfo<std::chrono::microseconds, void>::GetNumberOfFields()
+ std::size_t
+ DataFieldsInfo<std::chrono::microseconds, void>::GetNumberOfFields()
{
return 1;
}
- void DataFieldsInfo<std::chrono::microseconds, void>::GetDataFieldAs(std::size_t i, const std::chrono::microseconds& field, long& out)
+
+ void
+ DataFieldsInfo<std::chrono::microseconds, void>::GetDataFieldAs(
+ std::size_t i,
+ const std::chrono::microseconds& field,
+ long& out)
{
ARMARX_CHECK_EQUAL(i, 0);
out = field.count();
}
- void DataFieldsInfo<std::chrono::microseconds, void>::GetDataFieldAs(std::size_t i, const std::chrono::microseconds& field, std::string& out)
+
+ void
+ DataFieldsInfo<std::chrono::microseconds, void>::GetDataFieldAs(
+ std::size_t i,
+ const std::chrono::microseconds& field,
+ std::string& out)
{
ARMARX_CHECK_EQUAL(i, 0);
out = to_string(field.count());
}
- const std::type_info& GetDataFieldType(std::size_t i)
+
+ const std::type_info&
+ GetDataFieldType(std::size_t i)
{
return typeid(long);
}
- std::map<std::string, VariantBasePtr> DataFieldsInfo<std::chrono::microseconds, void>::ToVariants(
- std::chrono::microseconds value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent)
- {
- ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) << "There is no framed version of TimestampVariant";
+
+ std::map<std::string, VariantBasePtr>
+ DataFieldsInfo<std::chrono::microseconds, void>::ToVariants(std::chrono::microseconds value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent)
+ {
+ ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty())
+ << "There is no framed version of TimestampVariant";
return {{name, {new TimedVariant(TimestampVariant{value.count()}, timestamp)}}};
}
-}
+} // namespace armarx::introspection
+
//IceUtil::Time
namespace armarx::introspection
{
- std::size_t DataFieldsInfo<IceUtil::Time, void>::GetNumberOfFields()
+ std::size_t
+ DataFieldsInfo<IceUtil::Time, void>::GetNumberOfFields()
{
return 1;
}
- void DataFieldsInfo<IceUtil::Time, void>::GetDataFieldAs(std::size_t i, const IceUtil::Time& field, long& out)
+
+ void
+ DataFieldsInfo<IceUtil::Time, void>::GetDataFieldAs(std::size_t i,
+ const IceUtil::Time& field,
+ long& out)
{
ARMARX_CHECK_EQUAL(i, 0);
out = field.toMicroSeconds();
}
- void DataFieldsInfo<IceUtil::Time, void>::GetDataFieldAs(std::size_t i, const IceUtil::Time& field, std::string& out)
+
+ void
+ DataFieldsInfo<IceUtil::Time, void>::GetDataFieldAs(std::size_t i,
+ const IceUtil::Time& field,
+ std::string& out)
{
ARMARX_CHECK_EQUAL(i, 0);
out = to_string(field.toMicroSeconds());
}
- const std::type_info& DataFieldsInfo<IceUtil::Time, void>::GetDataFieldType(std::size_t i)
+
+ const std::type_info&
+ DataFieldsInfo<IceUtil::Time, void>::GetDataFieldType(std::size_t i)
{
return typeid(long);
}
- std::map<std::string, VariantBasePtr> DataFieldsInfo<IceUtil::Time, void>::ToVariants(
- IceUtil::Time value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent)
- {
- ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) << "There is no framed version of TimestampVariant";
+
+ std::map<std::string, VariantBasePtr>
+ DataFieldsInfo<IceUtil::Time, void>::ToVariants(IceUtil::Time value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent)
+ {
+ ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty())
+ << "There is no framed version of TimestampVariant";
return {{name, {new TimedVariant(TimestampVariant{value.toMicroSeconds()}, timestamp)}}};
}
-}
+} // namespace armarx::introspection
+
//JointStatus
namespace armarx::introspection
{
- std::size_t DataFieldsInfo<JointStatus, void>::GetNumberOfFields()
+ std::size_t
+ DataFieldsInfo<JointStatus, void>::GetNumberOfFields()
{
return 4;
}
- void DataFieldsInfo<JointStatus, void>::GetDataFieldAs(
- std::size_t i, const JointStatus& field, std::string& out)
+
+ void
+ DataFieldsInfo<JointStatus, void>::GetDataFieldAs(std::size_t i,
+ const JointStatus& field,
+ std::string& out)
{
ARMARX_CHECK_LESS(i, 4);
switch (i)
@@ -360,14 +447,14 @@ namespace armarx::introspection
out = to_string(field.emergencyStop);
return;
}
- throw std::logic_error
- {
- __FILE__ " : " + to_string(__LINE__) +
- " : Unreachable code reached"
- };
+ throw std::logic_error{__FILE__ " : " + to_string(__LINE__) +
+ " : Unreachable code reached"};
}
- void DataFieldsInfo<JointStatus, void>::GetDataFieldAs(
- std::size_t i, const JointStatus& field, Ice::Int& out)
+
+ void
+ DataFieldsInfo<JointStatus, void>::GetDataFieldAs(std::size_t i,
+ const JointStatus& field,
+ Ice::Int& out)
{
ARMARX_CHECK_EXPRESSION(i == 0 || i == 1);
switch (i)
@@ -379,14 +466,14 @@ namespace armarx::introspection
out = field.emergencyStop;
return;
}
- throw std::logic_error
- {
- __FILE__ " : " + to_string(__LINE__) +
- " : Unreachable code reached"
- };
+ throw std::logic_error{__FILE__ " : " + to_string(__LINE__) +
+ " : Unreachable code reached"};
}
- void DataFieldsInfo<JointStatus, void>::GetDataFieldAs(
- std::size_t i, const JointStatus& field, bool& out)
+
+ void
+ DataFieldsInfo<JointStatus, void>::GetDataFieldAs(std::size_t i,
+ const JointStatus& field,
+ bool& out)
{
ARMARX_CHECK_EXPRESSION(i == 2 || i == 3);
switch (i)
@@ -398,13 +485,12 @@ namespace armarx::introspection
out = static_cast<Ice::Int>(field.operation);
return;
}
- throw std::logic_error
- {
- __FILE__ " : " + to_string(__LINE__) +
- " : Unreachable code reached"
- };
+ throw std::logic_error{__FILE__ " : " + to_string(__LINE__) +
+ " : Unreachable code reached"};
}
- const std::type_info& DataFieldsInfo<JointStatus, void>::GetDataFieldType(std::size_t i)
+
+ const std::type_info&
+ DataFieldsInfo<JointStatus, void>::GetDataFieldType(std::size_t i)
{
ARMARX_CHECK_LESS(i, 4);
switch (i)
@@ -418,34 +504,31 @@ namespace armarx::introspection
case 3:
return typeid(bool);
}
- throw std::logic_error
- {
- __FILE__ " : " + to_string(__LINE__) +
- " : Unreachable code reached"
- };
+ throw std::logic_error{__FILE__ " : " + to_string(__LINE__) +
+ " : Unreachable code reached"};
}
- const std::vector<std::string>& DataFieldsInfo<JointStatus, void>::GetFieldNames()
+ const std::vector<std::string>&
+ DataFieldsInfo<JointStatus, void>::GetFieldNames()
{
- static const std::vector<std::string> result{"error", "operation", "enabled", "emergencyStop"};
+ static const std::vector<std::string> result{
+ "error", "operation", "enabled", "emergencyStop"};
return result;
}
- std::map<std::string, VariantBasePtr> DataFieldsInfo<JointStatus, void>::ToVariants(
- const JointStatus& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent)
- {
- ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) << "There is no framed version for JointStatus";
+ std::map<std::string, VariantBasePtr>
+ DataFieldsInfo<JointStatus, void>::ToVariants(const JointStatus& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent)
+ {
+ ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty())
+ << "There is no framed version for JointStatus";
static const auto fnames = GetFieldNames();
- return
- {
- {name + fnames.at(0), {new TimedVariant{to_string(value.error), timestamp}}},
- {name + fnames.at(1), {new TimedVariant{to_string(value.operation), timestamp}}},
- {name + fnames.at(2), {new TimedVariant{value.enabled, timestamp}}},
- {name + fnames.at(3), {new TimedVariant{value.emergencyStop, timestamp}}}
- };
+ return {{name + fnames.at(0), {new TimedVariant{to_string(value.error), timestamp}}},
+ {name + fnames.at(1), {new TimedVariant{to_string(value.operation), timestamp}}},
+ {name + fnames.at(2), {new TimedVariant{value.enabled, timestamp}}},
+ {name + fnames.at(3), {new TimedVariant{value.emergencyStop, timestamp}}}};
}
-}
+} // namespace armarx::introspection
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
index 34f53f63744ff7939c1cfcf94526052d0dad2d71..e0e3d736b50bb906077fd20adbc8ddab7bed13cc 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
@@ -21,19 +21,21 @@
*/
#pragma once
-#include "../EigenForwardDeclarations.h"
+#include <chrono>
+#include <string>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/util/CPPUtility/trace.h>
-#include <ArmarXCore/observers/variant/TimedVariant.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
+#include <ArmarXCore/observers/variant/TimedVariant.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
-#include <string>
-#include <chrono>
+#include "../EigenForwardDeclarations.h"
namespace armarx::introspection
{
- template<class T, class = void> struct DataFieldsInfo;
+ template <class T, class = void>
+ struct DataFieldsInfo;
+
// static std::size_t GetNumberOfFields();
// static void GetDataFieldAs(std::size_t i, T field, bool& out);
// static void GetDataFieldAs(std::size_t i, T field, Ice::Byte& out);
@@ -52,93 +54,115 @@ namespace armarx::introspection
// const std::string& frame = "",
// const std::string& agent = "")
- template<class T>
+ template <class T>
struct DataFieldsInfoBase
{
- static void GetDataFieldAs(std::size_t i, const T& field, bool& out)
+ static void
+ GetDataFieldAs(std::size_t i, const T& field, bool& out)
{
ARMARX_TRACE;
- throw std::logic_error {"This function should never be called"};
+ throw std::logic_error{"This function should never be called"};
}
- static void GetDataFieldAs(std::size_t i, const T& field, Ice::Byte& out)
+
+ static void
+ GetDataFieldAs(std::size_t i, const T& field, Ice::Byte& out)
{
ARMARX_TRACE;
- throw std::logic_error {"This function should never be called"};
+ throw std::logic_error{"This function should never be called"};
}
- static void GetDataFieldAs(std::size_t i, const T& field, Ice::Short& out)
+
+ static void
+ GetDataFieldAs(std::size_t i, const T& field, Ice::Short& out)
{
ARMARX_TRACE;
- throw std::logic_error {"This function should never be called"};
+ throw std::logic_error{"This function should never be called"};
}
- static void GetDataFieldAs(std::size_t i, const T& field, Ice::Int& out)
+
+ static void
+ GetDataFieldAs(std::size_t i, const T& field, Ice::Int& out)
{
ARMARX_TRACE;
- throw std::logic_error {"This function should never be called"};
+ throw std::logic_error{"This function should never be called"};
}
- static void GetDataFieldAs(std::size_t i, const T& field, Ice::Long& out)
+
+ static void
+ GetDataFieldAs(std::size_t i, const T& field, Ice::Long& out)
{
ARMARX_TRACE;
- throw std::logic_error {"This function should never be called"};
+ throw std::logic_error{"This function should never be called"};
}
- static void GetDataFieldAs(std::size_t i, const T& field, Ice::Float& out)
+
+ static void
+ GetDataFieldAs(std::size_t i, const T& field, Ice::Float& out)
{
ARMARX_TRACE;
- throw std::logic_error {"This function should never be called"};
+ throw std::logic_error{"This function should never be called"};
}
- static void GetDataFieldAs(std::size_t i, const T& field, Ice::Double& out)
+
+ static void
+ GetDataFieldAs(std::size_t i, const T& field, Ice::Double& out)
{
ARMARX_TRACE;
- throw std::logic_error {"This function should never be called"};
+ throw std::logic_error{"This function should never be called"};
}
- static void GetDataFieldAs(std::size_t i, const T& field, std::string& out)
+
+ static void
+ GetDataFieldAs(std::size_t i, const T& field, std::string& out)
{
ARMARX_TRACE;
- throw std::logic_error {"This function should never be called"};
+ throw std::logic_error{"This function should never be called"};
}
- static const std::vector<std::string>& GetFieldNames()
+
+ static const std::vector<std::string>&
+ GetFieldNames()
{
ARMARX_TRACE;
- throw std::logic_error {"This function should never be called"};
+ throw std::logic_error{"This function should never be called"};
}
};
-}
+} // namespace armarx::introspection
+
//build_in_ice_types
namespace armarx::introspection
{
-#define make_def_for_build_in_ice_type(Type) \
- template<> \
- struct DataFieldsInfo<Type, void> : DataFieldsInfoBase<Type> \
- { \
- using DataFieldsInfoBase<Type>::GetDataFieldAs; \
- static std::size_t GetNumberOfFields() \
- { \
- return 1; \
- } \
- static void GetDataFieldAs(std::size_t i, const Type& field, std::string& out) \
- { \
- ARMARX_CHECK_EQUAL(i, 0); \
- out = to_string(field); \
- } \
- static void GetDataFieldAs(std::size_t i, const Type& field, Type& out) \
- { \
- ARMARX_CHECK_EQUAL(i, 0); \
- out = field; \
- } \
- static const std::type_info& GetDataFieldType(std::size_t i) \
- { \
- return typeid(Type); \
- } \
- static std::map<std::string, VariantBasePtr> ToVariants( \
- const Type& value, \
- const std::string& name, \
- const IceUtil::Time& timestamp, \
- const std::string& frame = "", \
- const std::string& agent = "") \
- { \
- ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) \
- << "There is no framed version for build in ice types"; \
- return {{name, {new TimedVariant(value, timestamp)}}}; \
- } \
+#define make_def_for_build_in_ice_type(Type) \
+ template <> \
+ struct DataFieldsInfo<Type, void> : DataFieldsInfoBase<Type> \
+ { \
+ using DataFieldsInfoBase<Type>::GetDataFieldAs; \
+ static std::size_t \
+ GetNumberOfFields() \
+ { \
+ return 1; \
+ } \
+ static void \
+ GetDataFieldAs(std::size_t i, const Type& field, std::string& out) \
+ { \
+ ARMARX_CHECK_EQUAL(i, 0); \
+ out = to_string(field); \
+ } \
+ static void \
+ GetDataFieldAs(std::size_t i, const Type& field, Type& out) \
+ { \
+ ARMARX_CHECK_EQUAL(i, 0); \
+ out = field; \
+ } \
+ static const std::type_info& \
+ GetDataFieldType(std::size_t i) \
+ { \
+ return typeid(Type); \
+ } \
+ static std::map<std::string, VariantBasePtr> \
+ ToVariants(const Type& value, \
+ const std::string& name, \
+ const IceUtil::Time& timestamp, \
+ const std::string& frame = "", \
+ const std::string& agent = "") \
+ { \
+ ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) \
+ << "There is no framed version for build in ice types"; \
+ return {{name, {new TimedVariant(value, timestamp)}}}; \
+ } \
}
make_def_for_build_in_ice_type(bool);
make_def_for_build_in_ice_type(Ice::Byte);
@@ -148,11 +172,12 @@ namespace armarx::introspection
make_def_for_build_in_ice_type(Ice::Float);
make_def_for_build_in_ice_type(Ice::Double);
#undef make_def_for_build_in_ice_type
-}
+} // namespace armarx::introspection
+
//Eigen::Vector3f
namespace armarx::introspection
{
- template<>
+ template <>
struct DataFieldsInfo<Eigen::Vector3f, void> : DataFieldsInfoBase<Eigen::Vector3f>
{
using DataFieldsInfoBase<Eigen::Vector3f>::GetDataFieldAs;
@@ -161,60 +186,66 @@ namespace armarx::introspection
static void GetDataFieldAs(std::size_t i, const Eigen::Vector3f& field, std::string& out);
static const std::type_info& GetDataFieldType(std::size_t i);
static const std::vector<std::string>& GetFieldNames();
- static std::map<std::string, VariantBasePtr> ToVariants(
- const Eigen::Vector3f& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame = "",
- const std::string& agent = "");
+ static std::map<std::string, VariantBasePtr> ToVariants(const Eigen::Vector3f& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame = "",
+ const std::string& agent = "");
};
-}
+} // namespace armarx::introspection
+
//Eigen::Vector##Num##Type
namespace armarx::introspection
{
-#define make_DataFieldsInfo_for_eigen_vector(Type,TypeName,Num) \
- template<> \
- struct DataFieldsInfo<Eigen::Vector##Num##Type, void> : DataFieldsInfoBase<Eigen::Vector##Num##Type> \
- { \
- using DataFieldsInfoBase<Eigen::Vector##Num##Type>::GetDataFieldAs; \
- static std::size_t GetNumberOfFields() \
- { \
- return Num; \
- } \
- static void GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, std::string& out); \
- static void GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, Ice::TypeName& out); \
- static const std::type_info& GetDataFieldType(std::size_t i); \
- static const std::vector<std::string>& GetFieldNames(); \
- static std::map<std::string, VariantBasePtr> ToVariants( \
- const Eigen::Vector##Num##Type& value, \
- const std::string& name, \
- const IceUtil::Time& timestamp, \
- const std::string& frame = "", \
- const std::string& agent = ""); \
+#define make_DataFieldsInfo_for_eigen_vector(Type, TypeName, Num) \
+ template <> \
+ struct DataFieldsInfo<Eigen::Vector##Num##Type, void> : \
+ DataFieldsInfoBase<Eigen::Vector##Num##Type> \
+ { \
+ using DataFieldsInfoBase<Eigen::Vector##Num##Type>::GetDataFieldAs; \
+ static std::size_t \
+ GetNumberOfFields() \
+ { \
+ return Num; \
+ } \
+ static void \
+ GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, std::string& out); \
+ static void \
+ GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, Ice::TypeName& out); \
+ static const std::type_info& GetDataFieldType(std::size_t i); \
+ static const std::vector<std::string>& GetFieldNames(); \
+ static std::map<std::string, VariantBasePtr> \
+ ToVariants(const Eigen::Vector##Num##Type& value, \
+ const std::string& name, \
+ const IceUtil::Time& timestamp, \
+ const std::string& frame = "", \
+ const std::string& agent = ""); \
};
- make_DataFieldsInfo_for_eigen_vector(f, Float, 2)
- make_DataFieldsInfo_for_eigen_vector(f, Float, 4)
- make_DataFieldsInfo_for_eigen_vector(f, Float, 5)
- make_DataFieldsInfo_for_eigen_vector(f, Float, 6)
-
- make_DataFieldsInfo_for_eigen_vector(d, Double, 2)
- make_DataFieldsInfo_for_eigen_vector(d, Double, 3)
- make_DataFieldsInfo_for_eigen_vector(d, Double, 4)
- make_DataFieldsInfo_for_eigen_vector(d, Double, 5)
- make_DataFieldsInfo_for_eigen_vector(d, Double, 6)
-
- make_DataFieldsInfo_for_eigen_vector(i, Int, 2)
- make_DataFieldsInfo_for_eigen_vector(i, Int, 3)
- make_DataFieldsInfo_for_eigen_vector(i, Int, 4)
- make_DataFieldsInfo_for_eigen_vector(i, Int, 5)
- make_DataFieldsInfo_for_eigen_vector(i, Int, 6)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 2) make_DataFieldsInfo_for_eigen_vector(f,
+ Float,
+ 4)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 5)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 6)
+
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 2)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 3)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 4)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 5)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 6)
+
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 2)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 3)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 4)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 5)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 6)
#undef make_DataFieldsInfo_for_eigen_vector
-}
+} // namespace armarx::introspection
+
//Eigen::Matrix4f
namespace armarx::introspection
{
- template<>
+ template <>
struct DataFieldsInfo<Eigen::Matrix4f, void> : DataFieldsInfoBase<Eigen::Matrix4f>
{
using DataFieldsInfoBase<Eigen::Matrix4f>::GetDataFieldAs;
@@ -223,57 +254,61 @@ namespace armarx::introspection
static void GetDataFieldAs(std::size_t i, const Eigen::Matrix4f& field, std::string& out);
static const std::type_info& GetDataFieldType(std::size_t i);
static const std::vector<std::string>& GetFieldNames();
- static std::map<std::string, VariantBasePtr> ToVariants(
- const Eigen::Matrix4f& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame = "",
- const std::string& agent = "");
+ static std::map<std::string, VariantBasePtr> ToVariants(const Eigen::Matrix4f& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame = "",
+ const std::string& agent = "");
};
-}
+} // namespace armarx::introspection
+
//Eigen::Quaternionf
namespace armarx::introspection
{
- template<>
+ template <>
struct DataFieldsInfo<Eigen::Quaternionf, void> : DataFieldsInfoBase<Eigen::Quaternionf>
{
using DataFieldsInfoBase<Eigen::Quaternionf>::GetDataFieldAs;
static std::size_t GetNumberOfFields();
static void GetDataFieldAs(std::size_t i, const Eigen::Quaternionf& field, float& out);
- static void GetDataFieldAs(std::size_t i, const Eigen::Quaternionf& field, std::string& out);
+ static void
+ GetDataFieldAs(std::size_t i, const Eigen::Quaternionf& field, std::string& out);
static const std::type_info& GetDataFieldType(std::size_t i);
static const std::vector<std::string>& GetFieldNames();
- static std::map<std::string, VariantBasePtr> ToVariants(
- const Eigen::Quaternionf& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame = "",
- const std::string& agent = "");
+ static std::map<std::string, VariantBasePtr> ToVariants(const Eigen::Quaternionf& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame = "",
+ const std::string& agent = "");
};
-}
+} // namespace armarx::introspection
+
//std::chrono::microseconds
namespace armarx::introspection
{
- template<>
- struct DataFieldsInfo<std::chrono::microseconds, void> : DataFieldsInfoBase<std::chrono::microseconds>
+ template <>
+ struct DataFieldsInfo<std::chrono::microseconds, void> :
+ DataFieldsInfoBase<std::chrono::microseconds>
{
using DataFieldsInfoBase<std::chrono::microseconds>::GetDataFieldAs;
static std::size_t GetNumberOfFields();
- static void GetDataFieldAs(std::size_t i, const std::chrono::microseconds& field, long& out);
- static void GetDataFieldAs(std::size_t i, const std::chrono::microseconds& field, std::string& out);
+ static void
+ GetDataFieldAs(std::size_t i, const std::chrono::microseconds& field, long& out);
+ static void
+ GetDataFieldAs(std::size_t i, const std::chrono::microseconds& field, std::string& out);
static const std::type_info& GetDataFieldType(std::size_t i);
- static std::map<std::string, VariantBasePtr> ToVariants(
- std::chrono::microseconds value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame = "",
- const std::string& agent = "");
+ static std::map<std::string, VariantBasePtr> ToVariants(std::chrono::microseconds value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame = "",
+ const std::string& agent = "");
};
-}
+} // namespace armarx::introspection
+
//IceUtil::Time
namespace armarx::introspection
{
- template<>
+ template <>
struct DataFieldsInfo<IceUtil::Time, void> : DataFieldsInfoBase<IceUtil::Time>
{
using DataFieldsInfoBase<IceUtil::Time>::GetDataFieldAs;
@@ -281,22 +316,23 @@ namespace armarx::introspection
static void GetDataFieldAs(std::size_t i, const IceUtil::Time& field, long& out);
static void GetDataFieldAs(std::size_t i, const IceUtil::Time& field, std::string& out);
static const std::type_info& GetDataFieldType(std::size_t i);
- static std::map<std::string, VariantBasePtr> ToVariants(
- IceUtil::Time value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame = "",
- const std::string& agent = "");
+ static std::map<std::string, VariantBasePtr> ToVariants(IceUtil::Time value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame = "",
+ const std::string& agent = "");
};
-}
+} // namespace armarx::introspection
+
//JointStatus
namespace armarx
{
struct JointStatus;
}
+
namespace armarx::introspection
{
- template<>
+ template <>
struct DataFieldsInfo<JointStatus, void> : DataFieldsInfoBase<JointStatus>
{
using DataFieldsInfoBase<JointStatus>::GetDataFieldAs;
@@ -307,67 +343,79 @@ namespace armarx::introspection
static const std::type_info& GetDataFieldType(std::size_t i);
static const std::vector<std::string>& GetFieldNames();
- static std::map<std::string, VariantBasePtr> ToVariants(
- const JointStatus& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent);
+ static std::map<std::string, VariantBasePtr> ToVariants(const JointStatus& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent);
};
-}
+} // namespace armarx::introspection
+
//basic integral types
namespace armarx::introspection
{
-#define make_def_for_type_mapped_to_long(Type) \
- template<> \
- struct DataFieldsInfo<Type, void> : DataFieldsInfoBase<Type> \
- { \
- using DataFieldsInfoBase<Type>::GetDataFieldAs; \
- static std::size_t GetNumberOfFields() \
- { \
- return 1; \
- } \
- static void GetDataFieldAs(std::size_t i, const Type& field, std::string& out) \
- { \
- ARMARX_CHECK_EQUAL(i, 0); \
- out = to_string(field); \
- } \
- static void GetDataFieldAs(std::size_t i, const Type& field, Ice::Long& out) \
- { \
- ARMARX_CHECK_EQUAL(i, 0); \
- out = field; \
- } \
- static const std::type_info& GetDataFieldType(std::size_t i) \
- { \
- return typeid(Type); \
- } \
- static std::map<std::string, VariantBasePtr> ToVariants( \
- const Type& value, \
- const std::string& name, \
- const IceUtil::Time& timestamp, \
- const std::string& frame = "", \
- const std::string& agent = "") \
- { \
- ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) \
- << "There is no framed version for build in ice types"; \
- return {{name, {new TimedVariant(value, timestamp)}}}; \
- } \
+#define make_def_for_type_mapped_to_long(Type) \
+ template <> \
+ struct DataFieldsInfo<Type, void> : DataFieldsInfoBase<Type> \
+ { \
+ using DataFieldsInfoBase<Type>::GetDataFieldAs; \
+ static std::size_t \
+ GetNumberOfFields() \
+ { \
+ return 1; \
+ } \
+ static void \
+ GetDataFieldAs(std::size_t i, const Type& field, std::string& out) \
+ { \
+ ARMARX_CHECK_EQUAL(i, 0); \
+ out = to_string(field); \
+ } \
+ static void \
+ GetDataFieldAs(std::size_t i, const Type& field, Ice::Long& out) \
+ { \
+ ARMARX_CHECK_EQUAL(i, 0); \
+ out = field; \
+ } \
+ static const std::type_info& \
+ GetDataFieldType(std::size_t i) \
+ { \
+ return typeid(Type); \
+ } \
+ static std::map<std::string, VariantBasePtr> \
+ ToVariants(const Type& value, \
+ const std::string& name, \
+ const IceUtil::Time& timestamp, \
+ const std::string& frame = "", \
+ const std::string& agent = "") \
+ { \
+ ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) \
+ << "There is no framed version for build in ice types"; \
+ return {{name, {new TimedVariant(value, timestamp)}}}; \
+ } \
}
make_def_for_type_mapped_to_long(std::uint16_t);
make_def_for_type_mapped_to_long(std::uint32_t);
#undef make_def_for_type_mapped_to_long
-}
+} // namespace armarx::introspection
+
//std::array
namespace armarx::introspection
{
- template<class T, class = void>
- struct DataFieldsInfoHasNoFieldNames : std::false_type {};
+ template <class T, class = void>
+ struct DataFieldsInfoHasNoFieldNames : std::false_type
+ {
+ };
- template<class T>
-struct DataFieldsInfoHasNoFieldNames < T, std::enable_if_t < (&DataFieldsInfo<T>::GetFieldNames == &DataFieldsInfoBase<T>::GetFieldNames) >> : std::true_type {};
+ template <class T>
+ struct DataFieldsInfoHasNoFieldNames<
+ T,
+ std::enable_if_t<(&DataFieldsInfo<T>::GetFieldNames ==
+ &DataFieldsInfoBase<T>::GetFieldNames)>> : std::true_type
+ {
+ };
- template<class T>
+ template <class T>
static constexpr bool DataFieldsInfoHasNoFieldNamesV = DataFieldsInfoHasNoFieldNames<T>::value;
static_assert(!DataFieldsInfoHasNoFieldNamesV<Eigen::Vector3f>);
@@ -375,18 +423,21 @@ struct DataFieldsInfoHasNoFieldNames < T, std::enable_if_t < (&DataFieldsInfo<T>
static_assert(DataFieldsInfoHasNoFieldNamesV<Ice::Int>);
static_assert(DataFieldsInfoHasNoFieldNamesV<std::uint16_t>);
- template<class T, std::size_t N>
- struct DataFieldsInfo <std::array<T, N>, void> : DataFieldsInfoBase<std::array<T, N>>
+ template <class T, std::size_t N>
+ struct DataFieldsInfo<std::array<T, N>, void> : DataFieldsInfoBase<std::array<T, N>>
{
using sub_t = DataFieldsInfo<T>;
- static std::size_t GetNumberOfFields()
+
+ static std::size_t
+ GetNumberOfFields()
{
ARMARX_TRACE;
return N * sub_t::GetNumberOfFields();
}
- template<class OT>
- static void GetDataFieldAs(std::size_t i, const std::array<T, N>& field, OT& out)
+ template <class OT>
+ static void
+ GetDataFieldAs(std::size_t i, const std::array<T, N>& field, OT& out)
{
ARMARX_TRACE;
ARMARX_CHECK_LESS(i, GetNumberOfFields());
@@ -394,23 +445,26 @@ struct DataFieldsInfoHasNoFieldNames < T, std::enable_if_t < (&DataFieldsInfo<T>
sub_t::GetDataFieldAs(i % subN, field.at(i / subN), out);
}
- static const std::type_info& GetDataFieldType(std::size_t i)
+ static const std::type_info&
+ GetDataFieldType(std::size_t i)
{
ARMARX_TRACE;
return sub_t::GetDataFieldType(i % N);
}
- static const std::vector<std::string>& GetFieldNames()
+
+ static const std::vector<std::string>&
+ GetFieldNames()
{
ARMARX_TRACE;
static const std::vector<std::string> result = []
{
- if constexpr(!DataFieldsInfoHasNoFieldNamesV<T>)
+ if constexpr (!DataFieldsInfoHasNoFieldNamesV<T>)
{
ARMARX_TRACE;
const auto sub_names = sub_t::GetFieldNames();
std::vector<std::string> r;
r.reserve(N * sub_names.size());
- for (std::size_t i = 0; i < N; ++i)
+ for (std::size_t i = 0; i < N; ++i)
{
const std::string pre = "element_" + std::to_string(i) + '.';
for (const auto& name : sub_names)
@@ -425,7 +479,7 @@ struct DataFieldsInfoHasNoFieldNames < T, std::enable_if_t < (&DataFieldsInfo<T>
ARMARX_TRACE;
std::vector<std::string> r;
r.reserve(N);
- for (std::size_t i = 0; i < N; ++i)
+ for (std::size_t i = 0; i < N; ++i)
{
r.emplace_back("element_" + std::to_string(i));
}
@@ -434,19 +488,21 @@ struct DataFieldsInfoHasNoFieldNames < T, std::enable_if_t < (&DataFieldsInfo<T>
}();
return result;
}
- static std::map<std::string, VariantBasePtr> ToVariants(
- const std::array<T, N>& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame = "",
- const std::string& agent = "")
+
+ static std::map<std::string, VariantBasePtr>
+ ToVariants(const std::array<T, N>& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame = "",
+ const std::string& agent = "")
{
ARMARX_TRACE;
std::map<std::string, VariantBasePtr> result;
- for (std::size_t i = 0; i < N; ++i)
+ for (std::size_t i = 0; i < N; ++i)
{
const std::string pre = "element_" + std::to_string(i) + '.';
- for (const auto& [k, v] : sub_t::ToVariants(value.at(i), name, timestamp, frame, agent))
+ for (const auto& [k, v] :
+ sub_t::ToVariants(value.at(i), name, timestamp, frame, agent))
{
result[pre + k] = v;
}
@@ -454,4 +510,4 @@ struct DataFieldsInfoHasNoFieldNames < T, std::enable_if_t < (&DataFieldsInfo<T>
return result;
}
};
-}
+} // namespace armarx::introspection