diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
index adc615c85bd1fb9281e75e21fb91dea991c30c18..3ea01e7168761901edcebf9bc6639490f27fd8df 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
@@ -437,6 +437,11 @@ namespace armarx
return nextv;
}
+ PositionThroughVelocityControllerWithAccelerationBounds::PositionThroughVelocityControllerWithAccelerationBounds()
+ {
+ // buffer.set_capacity(10);
+ }
+
bool PositionThroughVelocityControllerWithAccelerationBounds::validParameters() const
{
return maxV > 0 &&
@@ -467,7 +472,9 @@ namespace armarx
const float posIfBrakingNow = currentPosition + brakingDistance;
const float posErrorIfBrakingNow = targetPosition - posIfBrakingNow;
const bool brakingNeeded = std::abs(brakingDistance) > std::abs(positionError);
- const float usedDeceleration = brakingNeeded ? std::abs(currentV * currentV / 2.f / positionError) : deceleration ;
+ const float usedDeceleration = (brakingNeeded && positionError != 0.0f) ?
+ std::abs(currentV * currentV / 2.f / positionError) :
+ deceleration;
const bool decelerate =
std::abs(currentV) > maxV || // we need to slow down (to stay in [-maxV,maxV]
brakingNeeded ||
@@ -475,7 +482,21 @@ namespace armarx
const float usedacc = decelerate ? -usedDeceleration : acceleration;
const float deltaVel = signV * usedacc * useddt;
float newTargetVel = boost::algorithm::clamp(currentV + deltaVel, -maxV, maxV);
- bool usePID = std::abs(newTargetVelPController) < std::abs(newTargetVel);
+ bool usePID = std::abs(newTargetVelPController) < std::abs(newTargetVel)
+ || std::abs(newTargetVelPController) < pControlVelLimit
+ || std::abs(positionError) < pControlPosErrorLimit;
+ // buffer.push_back({currentPosition, newTargetVelPController, newTargetVel, currentV, positionError});
+
+ // if (PIDModeActive != usePID)
+ // {
+ // for (auto& elem : buffer)
+ // {
+ // ARMARX_INFO << VAROUT(elem.currentPosition) << VAROUT(elem.targetVelocityPID) << VAROUT(elem.targetVelocityRAMP) << VAROUT(elem.currentV) << VAROUT(elem.currentError);
+ // }
+ // ARMARX_IMPORTANT << VAROUT(newTargetVelPController) << VAROUT(newTargetVel) << VAROUT(currentPosition) << VAROUT(usePID) << VAROUT(positionError);
+ // }
+ // ARMARX_INFO << deactivateSpam(1) << VAROUT(newTargetVelPController) << VAROUT(newTargetVel) << VAROUT(currentPosition) << VAROUT(usePID) << VAROUT(positionError) << VAROUT(targetPosition) << VAROUT(currentV) << VAROUT(deltaVel) << VAROUT(useddt) << VAROUT(usedacc);
+ // PIDModeActive = usePID;
// ARMARX_INFO << deactivateSpam(0.2) << VAROUT(usePID) << VAROUT(dt) << VAROUT(decelerate) << VAROUT(usedacc) << VAROUT(maxDeltaV) << VAROUT(deltaVel) << VAROUT(newTargetVel) << VAROUT(newTargetVelPController)
// << VAROUT(currentPosition) << VAROUT(targetPosition);
// ARMARX_INFO << VAROUT(hardBrakeNeeded) << VAROUT(usedacc) << VAROUT(currentV) << VAROUT(newTargetVel) << VAROUT(decelerate) << VAROUT(brakingDistance) << VAROUT(positionError);
@@ -689,6 +710,7 @@ namespace armarx
//run should be static, but we need to adapt the parabs for the subcontroler -> have the subcontroler as value;
PositionThroughVelocityControllerWithAccelerationBounds sub = *this; // slice parent class from this
sub.currentPosition = periodicClamp(currentPosition, positionPeriodLo, positionPeriodHi);
+ // ARMARX_INFO << deactivateSpam(1) << VAROUT(currentPosition) << VAROUT(sub.currentPosition) << VAROUT(positionPeriodLo) << VAROUT(positionPeriodHi);
sub.targetPosition = periodicClamp(targetPosition , positionPeriodLo, positionPeriodHi);
const float brakingDist = brakingDistance(currentV, deceleration);
diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.h b/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
index 035b8d215c3b4c7cd698e8905be48874bc87ca5d..c4235c90edfb958b0fda86d3bf76757bbaa47b8e 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
@@ -27,6 +27,7 @@
#include <type_traits>
#include <ArmarXCore/core/util/algorithm.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
+#include <boost/circular_buffer.hpp>
namespace armarx
{
@@ -87,43 +88,43 @@ namespace armarx
if (newV < std::abs(v0))
{
- dt0 = v0/dec;
+ dt0 = v0 / dec;
dt1 = newDt - dt0;
deltas d = accelerateToVelocity(v0, dec, 0);
- newV =(distance - d.dx) / dt1;
+ newV = (distance - d.dx) / dt1;
newAcc = dec;
newDec = dec;
-// // float oldDt = trapeze.at(0).dt + trapeze.at(1).dt + trapeze.at(2).dt;
-// // dt0 = trapeze.at(0).dt;
-// // dt1 = trapeze.at(1).dt;
-// // dt2 = trapeze.at(2).dt;
-// // area = trapezeArea(v0, newV, dt0, dt1, dt2);
-// // auto v_pair = pq(dt1*dec, -(area*dec+v0*v0*0.5f));
-// auto v_pair = pq(-newDt * dec - v0, area * dec - v0 * v0 * 0.5f);
-// if (std::isfinite(v_pair.first) && std::isfinite(v_pair.second))
-// {
-// newV = std::abs(v_pair.first) < std::abs(v_pair.second) ? v_pair.first : v_pair.second;
-// }
-// else if (std::isfinite(v_pair.first))
-// {
-// newV = v_pair.first;
-// }
-// else if (std::isfinite(v_pair.second))
-// {
-// newV = v_pair.second;
-// }
-// // float f = (v0+newV)/2*((newV-v0)/dec) + dt1*newV + (newV/dec)*newV*0.5;
-// dt0 = std::abs(newV - v0) / dec;
-// dt1 = (newDt - std::abs(newV - v0) / dec - (newV / dec));
-// dt2 = (newV / dec);
-
-// float f = (v0 + newV) / 2 * (std::abs(newV - v0) / dec);
-// f += (newDt - std::abs(newV - v0) / dec - (newV / dec)) * newV;
-// f += (newV / dec) * newV * 0.5;
-
-// newAcc = dec;
-// newDec = dec;
+ // // float oldDt = trapeze.at(0).dt + trapeze.at(1).dt + trapeze.at(2).dt;
+ // // dt0 = trapeze.at(0).dt;
+ // // dt1 = trapeze.at(1).dt;
+ // // dt2 = trapeze.at(2).dt;
+ // // area = trapezeArea(v0, newV, dt0, dt1, dt2);
+ // // auto v_pair = pq(dt1*dec, -(area*dec+v0*v0*0.5f));
+ // auto v_pair = pq(-newDt * dec - v0, area * dec - v0 * v0 * 0.5f);
+ // if (std::isfinite(v_pair.first) && std::isfinite(v_pair.second))
+ // {
+ // newV = std::abs(v_pair.first) < std::abs(v_pair.second) ? v_pair.first : v_pair.second;
+ // }
+ // else if (std::isfinite(v_pair.first))
+ // {
+ // newV = v_pair.first;
+ // }
+ // else if (std::isfinite(v_pair.second))
+ // {
+ // newV = v_pair.second;
+ // }
+ // // float f = (v0+newV)/2*((newV-v0)/dec) + dt1*newV + (newV/dec)*newV*0.5;
+ // dt0 = std::abs(newV - v0) / dec;
+ // dt1 = (newDt - std::abs(newV - v0) / dec - (newV / dec));
+ // dt2 = (newV / dec);
+
+ // float f = (v0 + newV) / 2 * (std::abs(newV - v0) / dec);
+ // f += (newDt - std::abs(newV - v0) / dec - (newV / dec)) * newV;
+ // f += (newV / dec) * newV * 0.5;
+
+ // newAcc = dec;
+ // newDec = dec;
}
@@ -195,8 +196,8 @@ namespace armarx
float currentPosition;
float positionLimitLoSoft;
float positionLimitHiSoft;
-// float positionLimitLoHard;
-// float positionLimitHiHard;
+ // float positionLimitLoHard;
+ // float positionLimitHiHard;
bool validParameters() const;
float run() const;
@@ -223,13 +224,24 @@ namespace armarx
float deceleration;
float currentPosition;
float targetPosition;
- // float pControlPosErrorLimit;
- // float pControlVelLimit;
+ float pControlPosErrorLimit = 0.01;
+ float pControlVelLimit = 0.001; // if target is below this threshold, PID controller will be used
float p;
-
+ PositionThroughVelocityControllerWithAccelerationBounds();
bool validParameters() const;
float run() const;
float estimateTime() const;
+ // mutable bool PIDModeActive = false;
+ // struct HelpStruct
+ // {
+ // float currentPosition;
+ // float targetVelocityPID;
+ // float targetVelocityRAMP;
+ // float currentV;
+ // float currentError;
+ // };
+
+ // mutable boost::circular_buffer<HelpStruct> buffer;
};
float positionThroughVelocityControlWithAccelerationBounds(float dt, float maxDt,
float currentV, float maxV,
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
index a1bde0ec4e2739cc75cc48fed18690f9af6b8259..08bf67eea28b64f39b33e046010d2b7ef455a51b 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
@@ -63,6 +63,11 @@ namespace armarx
*target = control;
}
inline virtual void rtPreActivateController() override
+ {
+
+ }
+
+ void reset()
{
control = (*sensor) * sensorToControlOnActivateFactor;
if (std::abs(control) < resetZeroThreshold)
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
index 358bdd97eea725b3c203d7520cb26a44d033d93c..90b93250b1e936dd5ac6537295b3e848085f136a 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
@@ -2474,7 +2474,7 @@ namespace armarx
{
bool collision = selfCollisionAvoidanceData.robot->getCollisionChecker()->checkCollision(selfCollisionAvoidanceData.collisionModels[it->first], selfCollisionAvoidanceData.collisionModels[it->second]);
anyCollision |= collision;
- distancesString += "---- " + it->first + " <-> " + it->second + " in Collsition: " + (collision ? "True" : "False") + "\n";
+ distancesString += "---- " + it->first + " <-> " + it->second + " in Collision: " + (collision ? "True" : "False") + "\n";
// distances.push_back(distance);
if (collision)
{
@@ -2581,7 +2581,7 @@ namespace armarx
try
{
robot = VirtualRobot::RobotIO::loadRobot(robotFileName, VirtualRobot::RobotIO::eFull);
- rtRobot = robot->clone(robot->getName());
+ rtRobot = VirtualRobot::RobotIO::loadRobot(robotFileName, VirtualRobot::RobotIO::eStructure);
rtRobot->setUpdateCollisionModel(false);
rtRobot->setUpdateVisualization(false);
rtRobot->setThreadsafe(false);
@@ -2652,7 +2652,7 @@ namespace armarx
emergencyStopMaster = new RobotUnitEmergencyStopMaster {this, getProperty<std::string>("EmergencyStopTopic").getValue()};
ManagedIceObjectPtr obj = ManagedIceObjectPtr::dynamicCast(emergencyStopMaster);
ARMARX_CHECK_EXPRESSION_W_HINT(obj, "RobotUnitEmergencyStopMaster");
- getArmarXManager()->addObject(obj, getProperty<std::string>("EmergencyStopMasterName").getValue());
+ getArmarXManager()->addObject(obj, getProperty<std::string>("EmergencyStopMasterName").getValue(), true);
auto prx = obj->getProxy(-1);
try
{
@@ -2734,8 +2734,9 @@ namespace armarx
{
node.first->setJointValueNoUpdate(node.second->position);
}
+ // TIMING_START(RtRobotUpdate);
rtRobot->applyJointValues();
-
+ // TIMING_END(RtRobotUpdate);
rtThreadTimingsSensorDevice->rtMarkRtUpdateSensorAndControlBufferEnd();
}
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h
index 04588cb5561bed5e9c6d17628c486c5ff0ac41b8..a25d71642e109892202c81a3063e1cb2498ebb8b 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h
@@ -35,12 +35,17 @@ namespace armarx
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
Eigen::Vector3f torque;
Eigen::Vector3f force;
+ Eigen::Vector3f gravityCompensatedTorque;
+ Eigen::Vector3f gravityCompensatedForce;
+
virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override
{
return
{
{"torque", {new TimedVariant{Vector3{torque}, timestamp}}},
{"force", {new TimedVariant{Vector3{force }, timestamp}}},
+ {"gravityCompensatedTorque", {new TimedVariant{Vector3{gravityCompensatedTorque}, timestamp}}},
+ {"gravityCompensatedForce", {new TimedVariant{Vector3{gravityCompensatedForce }, timestamp}}},
};
}
};
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
index 0899433b1f1c3f398d90a087d2be74d3b334b751..1cd5d65e4ef0ca4f5bafa04050bc182dfc26786a 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
@@ -21,6 +21,12 @@
*/
#include "KinematicSubUnit.h"
+armarx::KinematicSubUnit::KinematicSubUnit() :
+ reportSkipper(20.0f)
+{
+
+}
+
void armarx::KinematicSubUnit::setupData(
std::string relRobFile,
VirtualRobot::RobotPtr rob,
@@ -129,22 +135,25 @@ void armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc, const
<< motorCurrents.size() << " \tmotor currents (updated = " << motorCurrentAValueChanged << ")\n"
<< motorTemperatures.size() << " \tmotor temperatures (updated = " << motorTemperaturesAValueChanged << ")\n";
auto prx = listenerPrx->ice_batchOneway();
- prx->reportControlModeChanged(ctrlModes, timestamp, ctrlModesAValueChanged);
prx->reportJointAngles(ang, timestamp, angAValueChanged);
prx->reportJointVelocities(vel, timestamp, velAValueChanged);
- prx->reportJointAccelerations(acc, timestamp, accAValueChanged);
prx->reportJointTorques(tor, timestamp, torAValueChanged);
- if (!motorCurrents.empty())
- {
- prx->reportJointCurrents(motorCurrents, timestamp, motorCurrentAValueChanged);
- }
- if (!motorTemperatures.empty())
- {
- prx->reportJointMotorTemperatures(motorTemperatures, timestamp, motorTemperaturesAValueChanged);
- }
- if (!statuses.empty())
+ if (reportSkipper.checkFrequency("Meta")) // only report the following data with low frequency
{
- prx->reportJointStatuses(statuses, timestamp, statusesAvalueChanged);
+ prx->reportJointAccelerations(acc, timestamp, accAValueChanged);
+ if (!motorCurrents.empty())
+ {
+ prx->reportJointCurrents(motorCurrents, timestamp, motorCurrentAValueChanged);
+ }
+ if (!motorTemperatures.empty())
+ {
+ prx->reportJointMotorTemperatures(motorTemperatures, timestamp, motorTemperaturesAValueChanged);
+ }
+ if (!statuses.empty())
+ {
+ prx->reportJointStatuses(statuses, timestamp, statusesAvalueChanged);
+ }
+ prx->reportControlModeChanged(ctrlModes, timestamp, ctrlModesAValueChanged);
}
prx->ice_flushBatchRequests();
}
@@ -219,7 +228,7 @@ void armarx::KinematicSubUnit::switchControlMode(const armarx::NameControlModeMa
<< "'. (ignoring this device)";
continue;
}
- auto ctrl = devs.at(name).getController(mode);
+ NJointKinematicUnitPassThroughControllerPtr ctrl = NJointKinematicUnitPassThroughControllerPtr::dynamicCast(devs.at(name).getController(mode));
if (!ctrl)
{
ARMARX_WARNING << "requested unsupported mode " << mode << " for device '" << name
@@ -230,6 +239,7 @@ void armarx::KinematicSubUnit::switchControlMode(const armarx::NameControlModeMa
{
continue;
}
+ ctrl->reset(); // reset immediately instead of preActivate() to avoid race condition
toActivate[name] = std::move(ctrl);
}
const auto printToActivate = ARMARX_STREAM_PRINTER
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
index 926a6eba2b5b2cc2012d673a8e3740772843ce93..23695b566f8e0f71426d76e38f8343601d23531c 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
@@ -31,6 +31,8 @@
#include <RobotAPI/components/units/KinematicUnit.h>
+#include <ArmarXCore/core/util/IceReportSkipper.h>
+
#include "RobotUnitSubUnit.h"
#include "../util.h"
#include "../SensorValues/SensorValue1DoFActuator.h"
@@ -57,6 +59,7 @@ namespace armarx
NJointControllerPtr getController(ControlMode c) const;
NJointControllerPtr getActiveController() const;
};
+ KinematicSubUnit();
void setupData(
std::string relRobFile,
@@ -89,7 +92,7 @@ namespace armarx
private:
std::map<std::string, ActuatorData> devs;
// never use this when holding the mutex! (could cause deadlocks)
- RobotUnit* robotUnit;
+ RobotUnit* robotUnit = NULL;
mutable std::mutex dataMutex;
NameControlModeMap ctrlModes;
NameValueMap ang;
@@ -101,6 +104,7 @@ namespace armarx
NameStatusMap statuses;
std::vector<std::set<std::string>> controlDeviceHardwareControlModeGroups;
std::set<std::string> controlDeviceHardwareControlModeGroupsMerged;
+ IceReportSkipper reportSkipper;
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)
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index cc713d196386cf202bfd1acb7fde45f09b49ff3f..939beb04017ada1c6b77f91217c8415fef9d96e8 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -716,10 +716,9 @@ bool KinematicUnitWidgetController::initGUIComboBox(VirtualRobot::RobotNodeSetPt
QString name(rn[i]->getName().c_str());
ui.nodeListComboBox->addItem(name);
}
-
+ ui.nodeListComboBox->setCurrentIndex(-1);
return true;
}
-
return false;
}
diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp
index cea8fa2744f760265647b2d7f1ad18f5085733bb..12745ffe064e27a3d9d4c91b96882bcd313772ac 100644
--- a/source/RobotAPI/libraries/core/PIDController.cpp
+++ b/source/RobotAPI/libraries/core/PIDController.cpp
@@ -30,7 +30,7 @@
using namespace armarx;
-PIDController::PIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxDerivation, bool limitless, double limitlessLimitHi) :
+PIDController::PIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxDerivation, bool limitless) :
Kp(Kp),
Ki(Ki),
Kd(Kd),
@@ -43,8 +43,7 @@ PIDController::PIDController(float Kp, float Ki, float Kd, double maxControlValu
controlValueDerivation(0),
maxControlValue(maxControlValue),
maxDerivation(maxDerivation),
- limitless(limitless),
- limitlessLimitHi(limitlessLimitHi)
+ limitless(limitless)
{
reset();
}
@@ -103,7 +102,10 @@ void PIDController::update(double deltaSec, double measuredValue, double targetV
//ARMARX_INFO << VAROUT(target) << ", " << VAROUT(processValue) << ", " << VAROUT(error);
if (limitless)
{
- error = math::MathUtils::fmod(error, limitlessLimitHi - 2 * M_PI, limitlessLimitHi);
+ //ARMARX_INFO << VAROUT(error);
+ error = math::MathUtils::angleModPI(error);
+ //ARMARX_INFO << VAROUT(error);
+ //error = math::MathUtils::fmod(error, limitlessLimitHi - 2 * M_PI, limitlessLimitHi);
//ARMARX_INFO << "Limitless after mod:" << VAROUT(error);
}
diff --git a/source/RobotAPI/libraries/core/PIDController.h b/source/RobotAPI/libraries/core/PIDController.h
index 4e1f5528b4164f5800a585bb327861e74e2265b7..e3676c7590e6ded3e67233c352fa415ad6a89130 100644
--- a/source/RobotAPI/libraries/core/PIDController.h
+++ b/source/RobotAPI/libraries/core/PIDController.h
@@ -40,8 +40,7 @@ namespace armarx
float Kd,
double maxControlValue = std::numeric_limits<double>::max(),
double maxDerivation = std::numeric_limits<double>::max(),
- bool limitless = false,
- double limitlessLimitHi = 2 * M_PI);
+ bool limitless = false);
void update(double deltaSec, double measuredValue, double targetValue);
void update(double measuredValue, double targetValue);
void update(double measuredValue);
@@ -64,7 +63,6 @@ namespace armarx
double maxDerivation;
bool firstRun;
bool limitless;
- double limitlessLimitHi;
mutable RecursiveMutex mutex;
};
typedef boost::shared_ptr<PIDController> PIDControllerPtr;
diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
index 07530219cc7b65f415117e5cc66617ba1c4d6947..00497e1eb2fe692669d6e201cc9c2dcb8ff389d9 100644
--- a/source/RobotAPI/libraries/core/Trajectory.cpp
+++ b/source/RobotAPI/libraries/core/Trajectory.cpp
@@ -1470,43 +1470,15 @@ namespace armarx
{
// linear interpolation
- double distance = fabs(itNext->timestamp - itPrev->timestamp);
- double weightPrev = fabs(t - itNext->timestamp) / distance;
- double weightNext = fabs(itPrev->timestamp - t) / distance;
+ float f = math::MathUtils::ILerp(itPrev->timestamp, itNext->timestamp, t);
if (dimension < limitless.size() && limitless.at(dimension).enabled)
{
- double result = 0;
- double range = limitless.at(dimension).limitHi - limitless.at(dimension).limitLo;
- double dist1 = next - previous;
- double dist2 = next - (previous + range);
- double dist3 = next - (previous - range);
- //ARMARX_IMPORTANT << "LIMITLESS: checking dim " << dimension << ", prev:" << previous << ", next:" << next << ", range:" << range << ", dist1:" << dist1 << ", dist2:" << dist2 << ", dist3:" << dist3;
- if (fabs(dist1) <= fabs(dist2) && fabs(dist1) <= fabs(dist3))
- {
- // no issue with bounds
- result = weightNext * next + weightPrev * previous;
- }
- else if (fabs(dist2) <= fabs(dist3) && fabs(dist2) <= fabs(dist3))
- {
- // go over hi bound
- result = weightNext * next + weightPrev * (previous + range);
- result = math::MathUtils::fmod(result, limitless.at(dimension).limitLo, limitless.at(dimension).limitHi);
- //ARMARX_IMPORTANT << "LIMITLESS - HIGH: checking dim " << dimension << ": high bounds, result: " << result << ", orig result would be " << weightNext* next + weightPrev* previous;
- }
- else
- {
- // go over lo bound
- result = weightNext * next + weightPrev * (previous - range);
- result = math::MathUtils::fmod(result, limitless.at(dimension).limitLo, limitless.at(dimension).limitHi);
- //ARMARX_IMPORTANT << "LIMITLESS - LOW: checking dim " << dimension << ": low bounds, result: " << result << ", orig result would be " << weightNext* next + weightPrev* previous;
- }
- return result;
+ return math::MathUtils::AngleLerp(previous, next, f);
}
else
{
- //ARMARX_IMPORTANT << "LIMITLESS: dim " << dimension << ": not limitless, limitless.size()=" << limitless.size();
- return weightNext * next + weightPrev * previous;
+ return math::MathUtils::Lerp(previous, next, f);
}
}
}
diff --git a/source/RobotAPI/libraries/core/math/MathUtils.h b/source/RobotAPI/libraries/core/math/MathUtils.h
index eae0128a8508b7963ea5820394cc9a13cbf71173..4ea7d9a7f0c08ba80382c4caa4f0722c93b1057d 100644
--- a/source/RobotAPI/libraries/core/math/MathUtils.h
+++ b/source/RobotAPI/libraries/core/math/MathUtils.h
@@ -126,14 +126,12 @@ namespace armarx
static float fmod(float value, float boundLow, float boundHigh)
{
- if (value < 0)
+ value = std::fmod(value - boundLow, boundHigh - boundLow) + boundLow;
+ if (value < boundLow)
{
- return std::fmod(value + boundLow, boundHigh - boundLow) - boundLow;
- }
- else
- {
- return std::fmod(value - boundLow, boundHigh - boundLow) + boundLow;
+ value += boundHigh - boundLow;
}
+ return value;
}
static float angleMod2PI(float value)
@@ -143,9 +141,27 @@ namespace armarx
static float angleModPI(float value)
{
- return angleMod2PI(value) - M_PI;
+ return angleMod2PI(value + M_PI) - M_PI;
}
+ static float Lerp(float a, float b, float f)
+ {
+ return a * (1 - f) + b * f;
+ }
+
+ static float ILerp(float a, float b, float f)
+ {
+ return (f - a) / (b - a);
+ }
+
+ static float AngleLerp(float a, float b, float f)
+ {
+ b = fmod(b, a - M_PI, a + M_PI);
+ return Lerp(a, b, f);
+ }
+
+
+
};
}
}
diff --git a/source/RobotAPI/libraries/core/test/PIDControllerTest.cpp b/source/RobotAPI/libraries/core/test/PIDControllerTest.cpp
index 85ec430aa338836e18686887d51811e9ac3e53f1..2fc298fddc53062028ebf8febf556ac505b5fa02 100644
--- a/source/RobotAPI/libraries/core/test/PIDControllerTest.cpp
+++ b/source/RobotAPI/libraries/core/test/PIDControllerTest.cpp
@@ -39,3 +39,31 @@ BOOST_AUTO_TEST_CASE(PIDControllerTest)
}
+void check_close(float a, float b, float tolerance)
+{
+ if(fabs(a - b) > tolerance)
+ {
+ ARMARX_ERROR << "fabs(a-b) > tolerance. a=" << a << " b=" << b << " tolerance=" << tolerance;
+ BOOST_FAIL("");
+ }
+}
+
+BOOST_AUTO_TEST_CASE(PIDControllerContinousTest)
+{
+ PIDController c(1, 0, 0, 10, 10, true);
+
+ for(int a = -2; a <= 2; a++)
+ {
+ for(int b = -2; b <= 2; b++)
+ {
+ for(int t = -2; t <= 2; t++)
+ {
+ c.update(1, 0 + a*M_PI*2, t + b*M_PI*2);
+ check_close(c.getControlValue(), t, 0.01);
+
+ c.update(1, t + a*M_PI*2, 0 + b*M_PI*2);
+ check_close(c.getControlValue(), -t, 0.01);
+ }
+ }
+ }
+}