Verified Commit 8af9e37d authored by Justin Carpentier's avatar Justin Carpentier Committed by Justin Carpentier
Browse files

[Joint/All] Update calc_aba methods to take into account template Matrix6 input

parent 266ae5e3
......@@ -92,6 +92,7 @@ namespace se3
, S(nv)
, M(), v(), c()
, U(6,nv), Dinv(nv,nv), UDinv(6,nv)
, StU(nv,nv)
{}
/// \brief Vector of joints
......@@ -112,6 +113,8 @@ namespace se3
U_t U;
D_t Dinv;
UD_t UDinv;
D_t StU;
};
......@@ -136,6 +139,7 @@ namespace se3
typedef SE3Tpl<Scalar,Options> SE3;
typedef MotionTpl<Scalar,Options> Motion;
typedef InertiaTpl<Scalar,Options> Inertia;
typedef container::aligned_vector<JointModelVariant> JointModelVector;
......@@ -244,18 +248,19 @@ namespace se3
const Eigen::MatrixBase<ConfigVectorType> & qs,
const Eigen::MatrixBase<TangentVectorType> & vs) const;
void calc_aba(JointDataDerived & data, Inertia::Matrix6 & I, const bool update_I) const
template<typename Matrix6Like>
void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
{
data.U.noalias() = I * data.S;
Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> tmp (data.S.matrix().transpose() * data.U);
data.U.noalias() = I * data.S.matrix();
data.StU.noalias() = data.S.matrix().transpose() * data.U;
// compute inverse
data.Dinv.setIdentity();
tmp.llt().solveInPlace(data.Dinv);
data.StU.llt().solveInPlace(data.Dinv);
data.UDinv.noalias() = data.U * data.Dinv;
if (update_I)
I -= data.UDinv * data.U.transpose();
EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
}
Scalar finiteDifferenceIncrement() const
......
......@@ -271,15 +271,15 @@ namespace se3
data.v = vs.template segment<NV>(idx_v());
}
template<typename S2, int O2>
void calc_aba(JointDataDerived & data, Eigen::Matrix<S2,6,6,O2> & I, const bool update_I) const
template<typename Matrix6Like>
void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
{
data.U = I;
data.Dinv.setIdentity();
I.llt().solveInPlace(data.Dinv);
if (update_I)
I.setZero();
EIGEN_CONST_CAST(Matrix6Like,I).setZero();
}
Scalar finiteDifferenceIncrement() const
......
......@@ -198,8 +198,9 @@ namespace se3
const Eigen::MatrixBase<TangentVector> & v) const
{ calc_first_order(*this,data,q,v); }
void calc_aba(JointDataDerived & data, Inertia::Matrix6 & I, const bool update_I) const
{ ::se3::calc_aba(*this,data,I,update_I); }
template<typename Matrix6Like>
void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
{ ::se3::calc_aba(*this,data,EIGEN_CONST_CAST(Matrix6Like,I),update_I); }
std::string shortname() const { return ::se3::shortname(*this); }
static std::string classname() { return "JointModel"; }
......
......@@ -440,6 +440,8 @@ namespace se3
U_t U;
D_t Dinv;
UD_t UDinv;
D_t StU;
JointDataPlanarTpl () : M(1), U(), Dinv(), UDinv() {}
......@@ -503,23 +505,23 @@ namespace se3
data.v.m_theta_dot = q_dot(2);
}
template<typename S2, int O2>
void calc_aba(JointDataDerived & data, Eigen::Matrix<S2,6,6,O2> & I, const bool update_I) const
template<typename Matrix6Like>
void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
{
data.U.template leftCols<2>() = I.template leftCols<2>();
data.U.template rightCols<1>() = I.template rightCols<1>();
Eigen::Matrix<S2,3,3,O2> tmp;
tmp.template leftCols<2>() = data.U.template topRows<2>().transpose();
tmp.template rightCols<1>() = data.U.template bottomRows<1>();
data.StU.template leftCols<2>() = data.U.template topRows<2>().transpose();
data.StU.template rightCols<1>() = data.U.template bottomRows<1>();
// compute inverse
data.Dinv.setIdentity();
tmp.llt().solveInPlace(data.Dinv);
data.StU.llt().solveInPlace(data.Dinv);
data.UDinv.noalias() = data.U * data.Dinv;
if (update_I)
I -= data.UDinv * data.U.transpose();
EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
}
Scalar finiteDifferenceIncrement() const
......
......@@ -504,15 +504,15 @@ namespace se3
data.v.rate = v;
}
template<typename S2, int O2>
void calc_aba(JointDataDerived & data, Eigen::Matrix<S2,6,6,O2> & I, const bool update_I) const
template<typename Matrix6Like>
void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
{
data.U.noalias() = I.template block<6,3> (0,Inertia::LINEAR) * axis;
data.Dinv[0] = Scalar(1)/axis.dot(data.U.template segment <3> (Inertia::LINEAR));
data.UDinv.noalias() = data.U * data.Dinv;
if (update_I)
I -= data.UDinv * data.U.transpose();
EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
}
Scalar finiteDifferenceIncrement() const
......
......@@ -544,15 +544,15 @@ namespace se3
data.v.rate = v;
}
template<typename S2, int O2>
void calc_aba(JointDataDerived & data, Eigen::Matrix<S2,6,6,O2> & I, const bool update_I) const
template<typename Matrix6Like>
void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
{
data.U = I.col(Inertia::LINEAR + axis);
data.Dinv[0] = 1./I(Inertia::LINEAR + axis, Inertia::LINEAR + axis);
data.UDinv.noalias() = data.U * data.Dinv[0];
if (update_I)
I -= data.UDinv * data.U.transpose();
EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
}
Scalar finiteDifferenceIncrement() const
......
......@@ -518,15 +518,15 @@ namespace se3
data.v.w = (Scalar)vs[idx_v()];
}
template<typename S2, int O2>
void calc_aba(JointDataDerived & data, Eigen::Matrix<S2,6,6,O2> & I, const bool update_I) const
template<typename Matrix6Like>
void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
{
data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis;
data.Dinv[0] = (Scalar)(1)/axis.dot(data.U.template segment<3>(Motion::ANGULAR));
data.UDinv.noalias() = data.U * data.Dinv;
if (update_I)
I -= data.UDinv * data.U.transpose();
EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
}
Scalar finiteDifferenceIncrement() const
......
......@@ -136,15 +136,15 @@ namespace se3
data.v.w = (Scalar)vs[idx_v()];;
}
template<typename S2, int O2>
void calc_aba(JointDataDerived & data, Eigen::Matrix<S2,6,6,O2> & I, const bool update_I) const
template<typename Matrix6Like>
void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
{
data.U = I.col(Inertia::ANGULAR + axis);
data.Dinv[0] = (Scalar)(1)/I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis);
data.UDinv.noalias() = data.U * data.Dinv[0];
if (update_I)
I -= data.UDinv * data.U.transpose();
EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
}
Scalar finiteDifferenceIncrement() const
......
......@@ -614,15 +614,15 @@ namespace se3
data.v.w = (Scalar)vs[idx_v()];;
}
template<typename S2, int O2>
void calc_aba(JointDataDerived & data, Eigen::Matrix<S2,6,6,O2> & I, const bool update_I) const
template<typename Matrix6Like>
void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
{
data.U = I.col(Inertia::ANGULAR + axis);
data.Dinv[0] = Scalar(1)/I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis);
data.UDinv.noalias() = data.U * data.Dinv[0];
if (update_I)
I -= data.UDinv * data.U.transpose();
EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
}
Scalar finiteDifferenceIncrement() const
......
......@@ -304,6 +304,7 @@ namespace se3
U_t U;
D_t Dinv;
UD_t UDinv;
D_t StU;
JointDataSphericalZYXTpl () : M(1), U(), Dinv(), UDinv() {}
......@@ -392,22 +393,20 @@ namespace se3
data.c()(2) = -s1 * c2 * q_dot(0) * q_dot(1) - c1 * s2 * q_dot(0) * q_dot(2) - c2 * q_dot(1) * q_dot(2);
}
template<typename S2, int O2>
void calc_aba(JointDataDerived & data, Eigen::Matrix<S2,6,6,O2> & I, const bool update_I) const
template<typename Matrix6Like>
void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
{
typedef Eigen::Matrix<S2,3,3,O2> Matrix3;
data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * data.S.S_minimal;
Matrix3 tmp(data.S.S_minimal.transpose() * data.U.template middleRows<3>(Motion::ANGULAR));
data.StU.noalias() = data.S.S_minimal.transpose() * data.U.template middleRows<3>(Motion::ANGULAR);
// compute inverse
data.Dinv.setIdentity();
tmp.llt().solveInPlace(data.Dinv);
data.StU.llt().solveInPlace(data.Dinv);
data.UDinv.noalias() = data.U * data.Dinv;
if (update_I)
I -= data.UDinv * data.U.transpose();
EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
}
Scalar finiteDifferenceIncrement() const
......
......@@ -458,8 +458,8 @@ namespace se3
data.v() = vs.template segment<NV>(idx_v());
}
template<typename S2, int O2>
void calc_aba(JointDataDerived & data, Eigen::Matrix<S2,6,6,O2> & I, const bool update_I) const
template<typename Matrix6Like>
void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
{
data.U = I.template block<6,3>(0,Inertia::ANGULAR);
......@@ -468,14 +468,15 @@ namespace se3
data.U.template middleRows<3>(Inertia::ANGULAR).llt().solveInPlace(data.Dinv);
data.UDinv.template middleRows<3>(Inertia::ANGULAR).setIdentity(); // can be put in data constructor
data.UDinv.template middleRows<3>(Inertia::LINEAR) = data.U.template block<3,3>(Inertia::LINEAR, 0) * data.Dinv;
data.UDinv.template middleRows<3>(Inertia::LINEAR).noalias() = data.U.template block<3,3>(Inertia::LINEAR, 0) * data.Dinv;
if (update_I)
{
I.template block<3,3>(Inertia::LINEAR,Inertia::LINEAR)
-= data.UDinv.template middleRows<3>(Inertia::LINEAR) * I.template block<3,3> (Inertia::ANGULAR, Inertia::LINEAR);
I.template block<6,3>(0,Inertia::ANGULAR).setZero();
I.template block<3,3>(Inertia::ANGULAR,Inertia::LINEAR).setZero();
Matrix6Like & I_ = EIGEN_CONST_CAST(Matrix6Like,I);
I_.template block<3,3>(Inertia::LINEAR,Inertia::LINEAR)
-= data.UDinv.template middleRows<3>(Inertia::LINEAR) * I_.template block<3,3> (Inertia::ANGULAR, Inertia::LINEAR);
I_.template block<6,3>(0,Inertia::ANGULAR).setZero();
I_.template block<3,3>(Inertia::ANGULAR,Inertia::LINEAR).setZero();
}
}
......
......@@ -522,8 +522,8 @@ namespace se3
data.v() = this->jointVelocitySelector(vs);
}
template<typename S2, int O2>
void calc_aba(JointDataDerived & data, Eigen::Matrix<S2,6,6,O2> & I, const bool update_I) const
template<typename Matrix6Like>
void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
{
data.U = I.template middleCols<3>(Inertia::LINEAR);
......@@ -536,10 +536,11 @@ namespace se3
if (update_I)
{
I.template block<3,3>(Inertia::ANGULAR,Inertia::ANGULAR)
-= data.UDinv.template middleRows<3>(Inertia::ANGULAR) * I.template block<3,3>(Inertia::LINEAR, Inertia::ANGULAR);
I.template middleCols<3>(Inertia::LINEAR).setZero();
I.template block<3,3>(Inertia::LINEAR,Inertia::ANGULAR).setZero();
Matrix6Like & I_ = EIGEN_CONST_CAST(Matrix6Like,I);
I_.template block<3,3>(Inertia::ANGULAR,Inertia::ANGULAR)
-= data.UDinv.template middleRows<3>(Inertia::ANGULAR) * I_.template block<3,3>(Inertia::LINEAR, Inertia::ANGULAR);
I_.template middleCols<3>(Inertia::LINEAR).setZero();
I_.template block<3,3>(Inertia::LINEAR,Inertia::ANGULAR).setZero();
}
}
......
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