Commit a6f47b1c authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

remove multiply assignment operator from the implementation

parent 17897a88
......@@ -60,9 +60,7 @@ namespace pinocchio
{
SETTO,
ADDTO,
RMTO,
APPLY_ON_THE_LEFT,
APPLY_ON_THE_RIGHT
RMTO
};
......
......@@ -144,14 +144,6 @@ namespace pinocchio
lg1_.dIntegrate_dq(Q1(q), V1(v), J11(J),RMTO);
lg2_.dIntegrate_dq(Q2(q), V2(v), J22(J),RMTO);
break;
case APPLY_ON_THE_LEFT:
//TODO: Not Implemented Yet
assert(false && "Wrong Op requesed value");
break;
case APPLY_ON_THE_RIGHT:
//TODO: Not Implemented Yet
assert(false && "Wrong Op requesed value");
break;
default:
assert(false && "Wrong Op requesed value");
break;
......@@ -180,14 +172,6 @@ namespace pinocchio
lg1_.dIntegrate_dv(Q1(q), V1(v), J11(J),RMTO);
lg2_.dIntegrate_dv(Q2(q), V2(v), J22(J),RMTO);
break;
case APPLY_ON_THE_LEFT:
//TODO: Not Implemented Yet
assert(false && "Wrong Op requesed value");
break;
case APPLY_ON_THE_RIGHT:
//TODO: Not Implemented Yet
assert(false && "Wrong Op requesed value");
break;
default:
assert(false && "Wrong Op requesed value");
break;
......
......@@ -116,20 +116,6 @@ namespace pinocchio
Mout.template topRightCorner<2,1>() -= tinv;
Mout(2,2) -= (Scalar)1;
break;
case APPLY_ON_THE_LEFT:
// TODO: Aliasing here.
Mout.template topLeftCorner<2,2>() = R.transpose() * Mout.template topLeftCorner<2,2>();
Mout.template topRightCorner<2,1>() = R.transpose() * Mout.template topRightCorner<2,1>();
//No aliasing here.
Mout.template topLeftCorner<2,2>().noalias() += tinv * Mout.template bottomLeftCorner<1,2>();
Mout.template topRightCorner<2,1>().noalias() += tinv * Mout(2,2);
break;
case APPLY_ON_THE_RIGHT:
// TODO: Aliasing here.
Mout.template leftCols<2>() = Mout.template leftCols<2>() * R.transpose();
//No aliasing here.
Mout.template rightCols<1>().noalias() += Mout.template leftCols<2>() * tinv;
break;
default:
assert(false && "Wrong Op requesed value");
break;
......@@ -355,20 +341,6 @@ namespace pinocchio
Jout.template bottomLeftCorner<1,2>() -= Jtmp6.template bottomLeftCorner<1,2>();
Jout.template bottomRightCorner<1,1>() -= Jtmp6.template bottomRightCorner<1,1>();
break;
case APPLY_ON_THE_LEFT:
Jtmp6.template block<2, 1>(0,2) = Jtmp6.template topRightCorner<2,1>();
Jtmp6.template block<1, 2>(2,0) = Jtmp6.template bottomLeftCorner<1,2>();
Jtmp6(2,2) = Jtmp6(5,5);
//TODO: Aliasing here.
Jout = Jtmp6.template topLeftCorner<3,3>() * Jout;
break;
case APPLY_ON_THE_RIGHT:
Jtmp6.template block<2, 1>(0,2) = Jtmp6.template topRightCorner<2,1>();
Jtmp6.template block<1, 2>(2,0) = Jtmp6.template bottomLeftCorner<1,2>();
Jtmp6(2,2) = Jtmp6(5,5);
//TODO: Aliasing here.
Jout = Jout * Jtmp6.template topLeftCorner<3,3>();
break;
default:
assert(false && "Wrong Op requesed value");
break;
......@@ -620,12 +592,6 @@ namespace pinocchio
case RMTO:
Jout -= exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
break;
case APPLY_ON_THE_LEFT:
Jout = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose() * Jout;
break;
case APPLY_ON_THE_RIGHT:
Jout = Jout * exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
break;
default:
assert(false && "Wrong Op requesed value");
break;
......@@ -649,16 +615,10 @@ namespace pinocchio
case RMTO:
Jexp6<RMTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
break;
case APPLY_ON_THE_LEFT:
break;
case APPLY_ON_THE_RIGHT:
break;
default:
assert(false && "Wrong Op requesed value");
break;
}
}
// interpolate_impl use default implementation.
......
......@@ -189,12 +189,6 @@ namespace pinocchio
case RMTO:
Jout(0,0) -= 1;
break;
case APPLY_ON_THE_LEFT:
//Do Nothing
break;
case APPLY_ON_THE_RIGHT:
//Do Nothing
break;
default:
assert(false && "Wrong Op requesed value");
break;
......@@ -219,12 +213,6 @@ namespace pinocchio
case RMTO:
Jout(0,0) -= 1;
break;
case APPLY_ON_THE_LEFT:
//Do Nothing
break;
case APPLY_ON_THE_RIGHT:
//Do Nothing
break;
default:
assert(false && "Wrong Op requesed value");
break;
......@@ -427,14 +415,6 @@ namespace pinocchio
case RMTO:
Jout -= exp3(-v);
break;
case APPLY_ON_THE_LEFT:
//TODO: Improve implementation
Jout = exp3(-v) * Jout;
break;
case APPLY_ON_THE_RIGHT:
//TODO: Improve implementation
Jout = Jout * exp3(-v);
break;
default:
assert(false && "Wrong Op requesed value");
break;
......@@ -459,10 +439,6 @@ namespace pinocchio
case RMTO:
Jexp3<RMTO>(v, J.derived());
break;
case APPLY_ON_THE_LEFT:
break;
case APPLY_ON_THE_RIGHT:
break;
default:
assert(false && "Wrong Op requesed value");
break;
......
......@@ -121,12 +121,6 @@ namespace pinocchio
case RMTO:
Jout.diagonal().array() -= 1.;
break;
case APPLY_ON_THE_LEFT:
//Do Nothing
break;
case APPLY_ON_THE_RIGHT:
//Do Nothing
break;
default:
assert(false && "Wrong Op requesed value");
break;
......@@ -151,12 +145,6 @@ namespace pinocchio
case RMTO:
Jout.diagonal().array() -= 1.;
break;
case APPLY_ON_THE_LEFT:
//Do Nothing
break;
case APPLY_ON_THE_RIGHT:
//Do Nothing
break;
default:
assert(false && "Wrong Op requesed value");
break;
......
......@@ -156,10 +156,6 @@ namespace pinocchio
Jout(1,2) -= -b*r[0]; Jout(2,1) -= b*r[0];
Jout.noalias() -= c * r * r.transpose();
break;
case APPLY_ON_THE_LEFT:
break;
case APPLY_ON_THE_RIGHT:
break;
default:
assert(false && "Wrong Op requesed value");
break;
......@@ -181,7 +177,6 @@ namespace pinocchio
Jexp3<SETTO>(r, Jexp);
}
/** \brief Derivative of log3
*
* \param[in] theta the angle value.
......@@ -432,10 +427,6 @@ namespace pinocchio
- Jtmp3 * J;
break;
}
case APPLY_ON_THE_LEFT:
break;
case APPLY_ON_THE_RIGHT:
break;
default:
assert(false && "Wrong Op requesed value");
break;
......@@ -451,7 +442,6 @@ namespace pinocchio
Jexp6<SETTO>(nu, Jexp);
}
/** \brief Derivative of log6
* \f[
* \left(\begin{array}{cc}
......
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