Commit 8af9e37d by Justin Carpentier Committed by Justin Carpentier

### [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 SE3; typedef MotionTpl Motion; typedef InertiaTpl Inertia; typedef container::aligned_vector JointModelVector; ... ... @@ -244,18 +248,19 @@ namespace se3 const Eigen::MatrixBase & qs, const Eigen::MatrixBase & vs) const; void calc_aba(JointDataDerived & data, Inertia::Matrix6 & I, const bool update_I) const template void calc_aba(JointDataDerived & data, const Eigen::MatrixBase & I, const bool update_I) const { data.U.noalias() = I * data.S; Eigen::Matrix 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(idx_v()); } template void calc_aba(JointDataDerived & data, Eigen::Matrix & I, const bool update_I) const template void calc_aba(JointDataDerived & data, const Eigen::MatrixBase & 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 & 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 void calc_aba(JointDataDerived & data, const Eigen::MatrixBase & 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 void calc_aba(JointDataDerived & data, Eigen::Matrix & I, const bool update_I) const template void calc_aba(JointDataDerived & data, const Eigen::MatrixBase & 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 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 void calc_aba(JointDataDerived & data, Eigen::Matrix & I, const bool update_I) const template void calc_aba(JointDataDerived & data, const Eigen::MatrixBase & 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 void calc_aba(JointDataDerived & data, Eigen::Matrix & I, const bool update_I) const template void calc_aba(JointDataDerived & data, const Eigen::MatrixBase & 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 void calc_aba(JointDataDerived & data, Eigen::Matrix & I, const bool update_I) const template void calc_aba(JointDataDerived & data, const Eigen::MatrixBase & 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 void calc_aba(JointDataDerived & data, Eigen::Matrix & I, const bool update_I) const template void calc_aba(JointDataDerived & data, const Eigen::MatrixBase & 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 void calc_aba(JointDataDerived & data, Eigen::Matrix & I, const bool update_I) const template void calc_aba(JointDataDerived & data, const Eigen::MatrixBase & 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 void calc_aba(JointDataDerived & data, Eigen::Matrix & I, const bool update_I) const template void calc_aba(JointDataDerived & data, const Eigen::MatrixBase & I, const bool update_I) const { typedef Eigen::Matrix 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(idx_v()); } template void calc_aba(JointDataDerived & data, Eigen::Matrix & I, const bool update_I) const template void calc_aba(JointDataDerived & data, const Eigen::MatrixBase & 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 void calc_aba(JointDataDerived & data, Eigen::Matrix & I, const bool update_I) const template void calc_aba(JointDataDerived & data, const Eigen::MatrixBase & 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!