diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
index f4ddf940daca9fa453564ece6c98d258303eae21..64cf8d156839474447a7f0a18c615d7f9d4ddc2d 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
@@ -512,37 +512,22 @@ namespace armarx
curVel = 0;
curPos = posIfBrakingNow;
}
- //check for to high v
- if (std::abs(curVel) > maxV)
- {
- const float dv = curVel - maxV;
- t += dv / deceleration;
- curPos += 0.5f / deceleration * dv * dv * sign(curVel);
- }
+// //check for to high v
+// if (std::abs(curVel) > maxV)
+// {
+// const float dv = curVel - maxV;
+// float dt = dv / deceleration;
+// t += dt;
+// // curPos += 0.5f / deceleration * dv * dv * sign(curVel);
+// curPos += 0.5 * deceleration * dt * dt + curVel * dt * sign(curVel);
+// curVel = maxV;
+// }
//curBrake()<=curPosEr() && curVel <= maxV
- auto tr = trapeze(curVel, acceleration, maxV, deceleration, 0, curPosEr());
+ // auto tr = trapeze(curVel, 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;
- // //where are we if we acc to max and then dec
- // const deltas acc = accelerateToVelocity(curVel, acceleration, maxV);
- // const deltas dec = accelerateToVelocity(maxV , deceleration, 0);
- // const float dist = acc.dx + dec.dx;
- // if (std::abs(dist - curPosEr()) <= pControlPosErrorLimit)
- // {
- // //after this we are at the target
- // return t + acc.dt + dec.dt;
- // }
- // if (dist < curPosEr())
- // {
- // //calc how long to acc + dec
- // return t + acc.dt + dec.dt;
- // }
- // if (dist > curPosEr())
- // {
- // return t + acc.dt + dec.dt + (dist - curPosEr()) / maxV;
- // }
-
- // const float posErrorAfterBraking = curPosEr - curBrake;
- // return t;
+
}
@@ -602,7 +587,7 @@ namespace armarx
deltas mid;
mid.dx = dxMax - dx;
mid.dv = vMax;
- mid.dt = mid.dx / mid.dv;
+ mid.dt = std::abs(mid.dx / mid.dv);
return std::make_pair(true, std::array<deltas, 3> {dacc, mid, ddec});
}
//we need a lower vMax (vx)
@@ -641,7 +626,7 @@ namespace armarx
mid.dx = 0;
mid.dv = vx;
mid.dt = 0;
- return std::make_pair(false, std::array<deltas, 3> {daccvx, mid, ddecvx});
+ return std::make_pair(true, std::array<deltas, 3> {daccvx, mid, ddecvx});
}
case 2:
{
@@ -655,10 +640,10 @@ namespace armarx
if (daccvx1.dt + ddecvx1.dt < daccvx2.dt + ddecvx2.dt)
{
mid.dv = vxs.first;
- return std::make_pair(false, std::array<deltas, 3> {daccvx1, mid, ddecvx1});
+ return std::make_pair(true, std::array<deltas, 3> {daccvx1, mid, ddecvx1});
}
mid.dv = vxs.second;
- return std::make_pair(false, 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)"};
@@ -686,6 +671,13 @@ namespace armarx
}
}
+
+ std::array<deltas, 3> trapezeWithDt(float v0, float acc, float vMax, float dec, float vt, float dx, float dt)
+ {
+
+ }
+
+
float PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition::run() const
{
//run should be static, but we need to adapt the parabs for the subcontroler -> have the subcontroler as value;
diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.h b/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
index 12d6f5ef9185649016d3b31ab7382e5f630e23bc..9f8d71aa46c77aa640137ff944db1df0e7b9c830 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
@@ -62,13 +62,75 @@ namespace armarx
return d;
}
-
std::array<deltas, 3> trapeze(float v0, float acc, float vMax, float dec, float vt, float dx);
- // inline std::array<deltas, 3> trapezeWithDt(float v0, float acc, float vMax, float dec, float vt, float dx, float dt)
- // {
- // }
+ 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)
+ {
+
+
+ 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 area = trapezeArea(v0, vmax, dt0, dt1, dt2);
+ dt1 += newDt - oldDt;
+ newV = (-dt0 * v0 + 2 * area) / (dt0 + 2 * dt1 + dt2);
+ newAcc = (newV - v0) / dt0;
+ newDec = newV / dt2;
+
+ if (newV < std::abs(v0))
+ {
+ dt0 = v0/dec;
+ dt1 = newDt - dt0;
+ deltas d = accelerateToVelocity(v0, dec, 0);
+ 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;
+
+ }
+
+
+ }
+
+ std::array<deltas, 3> trapezeWithDt(float v0, float acc, float vMax, float dec, float vt, float dx, float dt);
/**
* @param v0 The initial velocity.
@@ -82,7 +144,7 @@ namespace armarx
inline float angleDistance(float angle1, float angle2)
{
- return M_PI - std::fabs(std::fmod(std::fabs(angle1 - angle2), M_PI*2) - M_PI);
+ return M_PI - std::fabs(std::fmod(std::fabs(angle1 - angle2), M_PI * 2) - M_PI);
}
struct VelocityControllerWithAccelerationBounds
@@ -161,8 +223,8 @@ namespace armarx
float deceleration;
float currentPosition;
float targetPosition;
-// float pControlPosErrorLimit;
-// float pControlVelLimit;
+ // float pControlPosErrorLimit;
+ // float pControlVelLimit;
float p;
bool validParameters() const;
diff --git a/source/RobotAPI/components/units/RobotUnit/test/SingleBasicControllerTest.cpp b/source/RobotAPI/components/units/RobotUnit/test/SingleBasicControllerTest.cpp
index 27436b07898fabfc65a0a53c2f6003b18d775276..3a752c201fbd9d9537396adbd4d78250709f5a58 100644
--- a/source/RobotAPI/components/units/RobotUnit/test/SingleBasicControllerTest.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/test/SingleBasicControllerTest.cpp
@@ -418,3 +418,39 @@ BOOST_AUTO_TEST_CASE(positionThroughVelocityControlWithAccelerationBoundsTest)
std::cout << "done positionThroughVelocityControlWithAccelerationBounds \n";
}
+BOOST_AUTO_TEST_CASE(timeEstimationTest)
+{
+ float val = std::nanf(std::to_string((1 << 16) - 1).c_str());
+ BOOST_CHECK(!(val > 0));
+ for (int i = 0; i < 10; i++)
+ {
+
+ ARMARX_INFO << "\nTrial " << i;
+ PositionThroughVelocityControllerWithAccelerationBounds c;
+ c.acceleration = 1;
+ c.deceleration = 2;
+ c.currentPosition = 0;
+ c.currentV = 0.5;
+ c.dt = 0.001;
+ c.maxDt = 0.002;
+ c.maxV = 1;
+ c.p = 1;
+ c.targetPosition = 2;
+ 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);
+
+ // c.maxV = 0.583;
+ // c.acceleration = 0.16666;
+ // c.deceleration = 1.1666;
+ ARMARX_INFO << VAROUT(c.maxV) << VAROUT(c.acceleration) << VAROUT(c.deceleration);
+ est = c.estimateTime();
+ ARMARX_INFO << "desired time: " << newDt << " estimated time " << est;
+ BOOST_CHECK_CLOSE(est, newDt, 1);
+ }
+
+}
+