Commit 92519469 authored by jcarpent's avatar jcarpent
Browse files

[Algo] Minor improvement of computation times for ABA

parent e93b3763
......@@ -86,7 +86,7 @@ namespace se3
jmodel.jointVelocitySelector(data.u) -= jdata.S().transpose()*data.f[i];
jmodel.calc_aba(jdata.derived(), Ia, parent > 0);
jmodel.jointVelocitySelector(data.ddq) = jdata.Dinv() * jmodel.jointVelocitySelector(data.u);
jmodel.jointVelocitySelector(data.ddq).noalias() = jdata.Dinv() * jmodel.jointVelocitySelector(data.u);
if (parent > 0)
{
......@@ -142,11 +142,18 @@ namespace se3
Block3 Co = res.block<3,3> (Inertia::ANGULAR, Inertia::LINEAR);
Block3 Do = res.block<3,3> (Inertia::ANGULAR, Inertia::ANGULAR);
Ao = R*Ai*R.transpose();
Bo = R*Bi*R.transpose();
Do.row(0) = t.cross(Bo.col(0));
Do.row(1) = t.cross(Bo.col(1));
Do.row(2) = t.cross(Bo.col(2));
Do.noalias() = R*Ai; // tmp variable
Ao.noalias() = Do*R.transpose();
Do.noalias() = R*Bi; // tmp variable
Bo.noalias() = Do*R.transpose();
Co.noalias() = R*Di; // tmp variable
Do.noalias() = Co*R.transpose();
Do.row(0) += t.cross(Bo.col(0));
Do.row(1) += t.cross(Bo.col(1));
Do.row(2) += t.cross(Bo.col(2));
Co.col(0) = t.cross(Ao.col(0));
Co.col(1) = t.cross(Ao.col(1));
......@@ -157,7 +164,6 @@ namespace se3
Do.col(0) += t.cross(Bo.col(0));
Do.col(1) += t.cross(Bo.col(1));
Do.col(2) += t.cross(Bo.col(2));
Do += R*Di*R.transpose();
return res;
}
};
......
......@@ -395,7 +395,7 @@ namespace se3
tmp.leftCols<2> () = data.U.topRows<2> ().transpose();
tmp.rightCols<1> () = data.U.bottomRows<1> ();
data.Dinv = tmp.inverse();
data.UDinv = data.U * data.Dinv;
data.UDinv.noalias() = data.U * data.Dinv;
if (update_I)
I -= data.UDinv * data.U.transpose();
......
......@@ -373,7 +373,7 @@ namespace se3
{
data.U = I.block<6,3> (0,Inertia::LINEAR) * axis;
data.Dinv[0] = 1./axis.dot(data.U.segment <3> (Inertia::LINEAR));
data.UDinv = data.U * data.Dinv;
data.UDinv.noalias() = data.U * data.Dinv;
if (update_I)
I -= data.UDinv * data.U.transpose();
......
......@@ -427,7 +427,7 @@ namespace se3
{
data.U = I.col(Inertia::LINEAR + axis);
data.Dinv[0] = 1./I(Inertia::LINEAR + axis, Inertia::LINEAR + axis);
data.UDinv = data.U * data.Dinv[0];
data.UDinv.noalias() = data.U * data.Dinv[0];
if (update_I)
I -= data.UDinv * data.U.transpose();
......
......@@ -377,7 +377,7 @@ namespace se3
{
data.U = I.block<6,3> (0,Inertia::ANGULAR) * axis;
data.Dinv[0] = 1./axis.dot(data.U.segment <3> (Inertia::ANGULAR));
data.UDinv = data.U * data.Dinv;
data.UDinv.noalias() = data.U * data.Dinv;
if (update_I)
I -= data.UDinv * data.U.transpose();
......
......@@ -124,7 +124,7 @@ namespace se3
{
data.U = I.col(Inertia::ANGULAR + axis);
data.Dinv[0] = 1./I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis);
data.UDinv = data.U * data.Dinv[0];
data.UDinv.noalias() = data.U * data.Dinv[0];
if (update_I)
I -= data.UDinv * data.U.transpose();
......
......@@ -467,7 +467,7 @@ namespace se3
{
data.U = I.col(Inertia::ANGULAR + axis);
data.Dinv[0] = 1./I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis);
data.UDinv = data.U * data.Dinv[0];
data.UDinv.noalias() = data.U * data.Dinv[0];
if (update_I)
I -= data.UDinv * data.U.transpose();
......
......@@ -467,7 +467,7 @@ namespace se3
data.U = I.middleCols<3> (Inertia::ANGULAR) * data.S.matrix();
Inertia::Matrix3 tmp (data.S.matrix().transpose() * data.U.middleRows<3> (Inertia::ANGULAR));
data.Dinv = tmp.inverse();
data.UDinv = data.U * data.Dinv;
data.UDinv.noalias() = data.U * data.Dinv;
if (update_I)
I -= data.UDinv * data.U.transpose();
......
Markdown is supported
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