Commit 6386df15 authored by Mehdi Benallegue's avatar Mehdi Benallegue
Browse files

Fully support the case of non planar feet case in ZMP from forces.

The added terms should be negligible in most cases, even when feet are not planar, but this version is more general.
parent 0eec5461
......@@ -80,7 +80,6 @@ namespace sot {
private:
Vector& computeZmp (Vector& zmp, int time)
{
double fz [footNumber];
double fnormal = 0;
double sumZmpx = 0;
double sumZmpy = 0;
......@@ -95,15 +94,19 @@ namespace sot {
return zmp;
}
const MatrixHomogeneous& M = sensorPositionsSIN_ [i]->access (time);
fz [i] = M (2,0) * f (0) + M(2,1) * f (1) + M (2,2) * f (2);
if (fz[i] > 0) {
/// we assume M (2,3)==0 for moments computations
double Mx = M (0,0)*f(3) + M (0,1)*f(4) + M (0,2)*f(5) + M (1,3)*(fz [i]);
double My = M (1,0)*f(3) + M (1,1)*f(4) + M (1,2)*f(5) - M (0,3)*(fz [i]);
fnormal += fz[i];
double fx = M (0,0) * f (0) + M(0,1) * f (1) + M (0,2) * f (2);
double fy = M (1,0) * f (0) + M(1,1) * f (1) + M (1,2) * f (2);
double fz = M (2,0) * f (0) + M(2,1) * f (1) + M (2,2) * f (2);
if (fz > 0) {
double Mx = M (0,0)*f(3) + M (0,1)*f(4) + M (0,2)*f(5)
+ M (1,3)*fz - M (2,3)*fy;
double My = M (1,0)*f(3) + M (1,1)*f(4) + M (1,2)*f(5)
+ M (2,3)*fx - M (0,3)*fz;
fnormal += fz;
sumZmpx -= My;
sumZmpy += Mx;
sumZmpz += fz [i] * M (2,3);
sumZmpz += fz * M (2,3);
}
}
if (fnormal != 0) {
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment