diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
index bd0acc86a97481e879929c6156a50f52cab0fd24..996f653e2e0772385606c3a7fb4abaafdbd64ba3 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
@@ -1183,6 +1183,184 @@ 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);
+ double accAtEnd = ctrlutil::a(remainingTime, currentAcc, jerk);
+
+ std::vector<State> states;
+ std::vector<State> states2;
+ //#define debug
+#ifdef debug
+ {
+ double dt = 0.001;
+ double t = 1;
+ double simS = currentPosition, simV = currentV, simAcc = currentAcc;
+ while (t > 0)
+ {
+ double jerk = min_jerk_calc_jerk(t, simS, simV, simAcc, targetPosition);
+ simAcc = simAcc + jerk * dt;
+ simV = ctrlutil::v(dt, simV, simAcc, 0);
+ simS += dt * simV;
+ t -= dt;
+ states.push_back(State{t, simS, simV, simAcc, jerk});
+ }
+ }
+ {
+ double dt = 0.001;
+
+ double t = 1;
+ double simS = currentPosition, simV = currentV, simAcc = currentAcc;
+ while (t > 0)
+ {
+ double jerk = min_jerk_calc_jerk(t, simS, simV, simAcc, targetPosition);
+ simS += ctrlutil::s(dt, 0, simV, simAcc, jerk);
+ simV = ctrlutil::v(dt, simV, simAcc, jerk);
+ simAcc = simAcc + jerk * dt;
+ t -= dt;
+ states2.push_back(State{t, simS, simV, simAcc, jerk});
+ }
+ }
+#endif
+ ARMARX_DEBUG << VAROUT(currentTime) << VAROUT(remainingTime) << VAROUT(jerk) << VAROUT(accAtEnd) << VAROUT(signedDistance) << " delta acc: " << jerk* dt;
+ currentTime += dt;
+
+
+
+ Output result { newVelocity, newAcc, jerk};
+ return result;
+
+ }
+
+ void MinJerkPositionController::reset()
+ {
+ currentTime = 0;
+
+ fixedMinJerkState.reset();
+// pid.reset();
+ currentP_Phase2 = -1;
+ currentP_Phase3 = -1;
+ }
+
+ double MinJerkPositionController::getTargetPosition() const
+ {
+ return targetPosition;
+ }
+
+ void MinJerkPositionController::setTargetPosition(double newTargetPosition)
+ {
+ if (std::abs(targetPosition - newTargetPosition) > 0.0001)
+ {
+ reset();
+ double distance = newTargetPosition - currentPosition;
+
+ 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) > distance)
+ // && ctrlutil::brakingDistance(currentV, currentAcc, desiredJerk) < distance)
+ {
+ finishTime = decelerationTime;
+ }
+ else
+ {
+ double accelerationTime = std::abs((math::MathUtils::Sign(distance)*maxV-currentV) / desiredDeceleration);
+ double decelerationTime = std::abs(maxV / desiredDeceleration);
+ finishTime = std::max(decelerationTime+accelerationTime, std::abs(distance / (maxV*0.75)));
+ }
+ targetPosition = newTargetPosition;
+ ARMARX_INFO << "New finish time: " << finishTime;
+ }
+
+
+ }
+
+ MinJerkPositionController::Output MinJerkPositionController::run()
+ {
+ 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)
+ {
+ double D = tf - t0;
+ BOOST_ASSERT(D != 0.0);
+ double D2 = D * D;
+ double tau = (t - t0) / D;
+ double b0 = s0;
+ double b1 = D * v0;
+ double b2 = D2 * a0 / 2;
+
+ double b3 = (-3 * D2) / 2 * a0 - 6 * D * v0 + 10 * (xf - s0);
+ double b4 = (3 * D2) / 2 * a0 + 8 * D * v0 - 15 * (xf - s0);
+ double b5 = (-D2) / 2 * a0 - 3 * D * v0 + 6 * (xf - s0);
+
+ double tau2 = tau * tau;
+ double tau3 = tau2 * tau;
+ 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 at = 2*b2 / D2 + 6*b3 / D2 * tau + 12*b4 / D2 * tau2 + 20*b5 / D2 * tau3;
+ return State{t, st, vt, at};
+ };
+
+ double remainingTime = finishTime - currentTime;
+ double signedDistance = targetPosition - currentPosition;
+ if (remainingTime <= 0.0)
+ {
+// if(!pid)
+// {
+// double Kd = (currentV - distance * p) /(-currentV);
+// ARMARX_INFO << VAROUT(Kd);
+// pid.reset(new PIDController(1, 0, Kd));
+// }
+ if(currentP_Phase3 < 0)
+ {
+ currentP_Phase3 = std::abs(currentV / signedDistance);
+ ARMARX_DEBUG << "optimal P: " << currentP_Phase3;
+ }
+
+
+ currentP_Phase3 = currentP_Phase3 * p_adjust_ratio + p * (1.0-p_adjust_ratio);
+ Output result {signedDistance * currentP_Phase3, 0, 0};
+ currentTime += dt;
+// State s = min_jerk(currentTime, finishTime, fixedMinJerkState->s0, fixedMinJerkState->v0, fixedMinJerkState->a0, targetPosition, fixedMinJerkState->t0);
+ ARMARX_DEBUG << 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(!fixedMinJerkState)
+ {
+ fixedMinJerkState.reset(new FixedMinJerkState{currentTime, currentPosition, currentV, currentAcc});
+ currentP_Phase2 = 0;
+ }
+// std::vector<State> states;
+// double t = currentTime;
+// while(t < finishTime)
+// {
+// State s = min_jerk(t, finishTime, fixedMinJerkState->s0, fixedMinJerkState->v0, fixedMinJerkState->a0, targetPosition, fixedMinJerkState->t0);
+// t += dt;
+// states.push_back(s);
+// }
+ currentTime += dt;
+ State s = min_jerk(currentTime, finishTime, fixedMinJerkState->s0, fixedMinJerkState->v0, fixedMinJerkState->a0, targetPosition, fixedMinJerkState->t0);
+ currentP_Phase2 = currentP_Phase2 * p_adjust_ratio + p * (1.0-p_adjust_ratio);
+ double newVelocity = (s.s - currentPosition) * currentP_Phase2 + s.v;
+ ARMARX_DEBUG << VAROUT(s.s) << VAROUT(s.v) << VAROUT(newVelocity);
+ Output result { newVelocity, 0, 0};
+ return result;
+ }
+
+
+
+
double jerk = min_jerk_calc_jerk(remainingTime, currentPosition, currentV, currentAcc, targetPosition);
double newAcc = currentAcc + jerk * dt;
double newVelocity = ctrlutil::v(dt, currentV, currentAcc, jerk);