diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp index a9c836515a8d3940ce2d18cc1a4f1603cb2e5773..6a978e80120f45608b2c54685653ee90bf3868a1 100644 --- a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp +++ b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp @@ -44,11 +44,16 @@ void PlatformUnitSimulation::onInitPlatformUnit() maxLinearVelocity = getProperty<float>("LinearVelocity").getValue(); maxAngularVelocity = getProperty<float>("AngularVelocity").getValue(); + velPID.reset(new MultiDimPIDController(getProperty<float>("Kp_Velocity").getValue(), 0, 0, getProperty<float>("MaxLinearAcceleration").getValue())); + posPID.reset(new MultiDimPIDController(getProperty<float>("Kp_Position").getValue(), 0, 0, maxLinearVelocity, getProperty<float>("MaxLinearAcceleration").getValue())); + + positionalAccuracy = 0.01; intervalMs = getProperty<int>("IntervalMs").getValue(); ARMARX_VERBOSE << "Starting platform unit simulation with " << intervalMs << " ms interval"; simulationTask = new PeriodicTask<PlatformUnitSimulation>(this, &PlatformUnitSimulation::simulationFunction, intervalMs, false, "PlatformUnitSimulation"); + } void PlatformUnitSimulation::onStartPlatformUnit() @@ -78,8 +83,9 @@ void PlatformUnitSimulation::onExitPlatformUnit() void PlatformUnitSimulation::simulationFunction() { // the time it took until this method was called again - double timeDeltaInSeconds = (TimeUtil::GetTime() - lastExecutionTime).toMilliSecondsDouble() / 1000.0; - lastExecutionTime = TimeUtil::GetTime(); + auto now = TimeUtil::GetTime(); + double timeDeltaInSeconds = (now - lastExecutionTime).toSecondsDouble(); + lastExecutionTime = now; std::vector<float> vel(3, 0.0f); { ScopedLock lock(currentPoseMutex); @@ -87,26 +93,16 @@ void PlatformUnitSimulation::simulationFunction() { case ePositionControl: { - float diff = fabs(targetPositionX - currentPositionX); - ARMARX_INFO << deactivateSpam(0.5) << "Distance to goal X: " << diff; - - if (diff > positionalAccuracy) - { - int sign = copysignf(1.0f, (targetPositionX - currentPositionX)); - currentPositionX += sign * std::min<float>(maxLinearVelocity * timeDeltaInSeconds, diff); - vel[0] = linearVelocityX; - } + posPID->update(timeDeltaInSeconds, + Eigen::Vector2f(currentPositionX, currentPositionY), + Eigen::Vector2f(targetPositionX, targetPositionY)); + float newXTickMotion = posPID->getControlValue()[0] * timeDeltaInSeconds; + currentPositionX += newXTickMotion; + currentPositionY += posPID->getControlValue()[1] * timeDeltaInSeconds; + vel[0] = posPID->getControlValue()[0]; + vel[1] = posPID->getControlValue()[1]; - diff = fabs(targetPositionY - currentPositionY); - - if (diff > positionalAccuracy) - { - int sign = copysignf(1.0f, (targetPositionY - currentPositionY)); - currentPositionY += sign * std::min<float>(maxLinearVelocity * timeDeltaInSeconds, diff); - vel[1] = linearVelocityY; - } - - diff = fabs(targetRotation - currentRotation); + float diff = fabs(targetRotation - currentRotation); if (diff > orientationalAccuracy) { @@ -132,10 +128,14 @@ void PlatformUnitSimulation::simulationFunction() case eVelocityControl: { Eigen::Vector2f targetVel(linearVelocityX, linearVelocityY); + velPID->update(timeDeltaInSeconds, currentTranslationVelocity, targetVel); + currentTranslationVelocity += timeDeltaInSeconds * velPID->getControlValue(); + currentPositionX += timeDeltaInSeconds * currentTranslationVelocity[0]; + currentPositionY += timeDeltaInSeconds * currentTranslationVelocity[1]; + + Eigen::Rotation2Df rot(currentRotation); targetVel = rot * targetVel; - currentPositionX += timeDeltaInSeconds * targetVel[0]; - currentPositionY += timeDeltaInSeconds * targetVel[1]; currentRotation += angularVelocity * timeDeltaInSeconds; // stay between +/- M_2_PI @@ -148,8 +148,8 @@ void PlatformUnitSimulation::simulationFunction() { currentRotation += 2 * M_PI; } - vel[0] = linearVelocityY; - vel[1] = linearVelocityY; + vel[0] = currentTranslationVelocity[0]; + vel[1] = currentTranslationVelocity[1]; vel[2] = angularVelocity; } break; diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.h b/source/RobotAPI/components/units/PlatformUnitSimulation.h index 71444589c8277c6cb4cdc1b35a0d96091b228847..8ebf0526b9396365b7da24e919c64a59b0cc1db5 100644 --- a/source/RobotAPI/components/units/PlatformUnitSimulation.h +++ b/source/RobotAPI/components/units/PlatformUnitSimulation.h @@ -29,6 +29,7 @@ #include <ArmarXCore/core/services/tasks/PeriodicTask.h> #include <ArmarXCore/core/system/ImportExportComponent.h> +#include <RobotAPI/libraries/core/PIDController.h> #include <IceUtil/Time.h> @@ -53,6 +54,9 @@ namespace armarx defineOptionalProperty<float>("InitialPosition.y", 0, "Initial y position of the platform."); defineOptionalProperty<std::string>("ReferenceFrame", "Platform", "Reference frame in which the platform position is reported."); defineOptionalProperty<float>("LinearVelocity", 1000.0, "Linear velocity of the platform (default: 1000 mm/sec)."); + defineOptionalProperty<float>("MaxLinearAcceleration", 1000.0, "Linear acceleration of the platform (default: 1000 mm/sec)."); + defineOptionalProperty<float>("Kp_Position", 5.0, "P value of the PID position controller"); + defineOptionalProperty<float>("Kp_Velocity", 5.0, "P value of the PID velocity controller"); defineOptionalProperty<float>("AngularVelocity", 1.5, "Angular velocity of the platform (default: 1.5 rad/sec)."); } }; @@ -125,6 +129,8 @@ namespace armarx ::Ice::Float orientationalAccuracy; std::string referenceFrame; + MultiDimPIDControllerPtr velPID, posPID; + Eigen::Vector2f currentTranslationVelocity = Eigen::Vector2f::Zero(); PeriodicTask<PlatformUnitSimulation>::pointer_type simulationTask;