Skip to content
Snippets Groups Projects
Commit 8a4c78cb authored by asantamaria's avatar asantamaria
Browse files

Commented PINV stuff to work in RR laptop

parent 0bda60f7
No related branches found
No related tags found
No related merge requests found
......@@ -29,9 +29,10 @@ Eigen::MatrixXd CCommon_Fc::WeightInvJac(const Eigen::MatrixXd& J, const Eigen::
}
// weighting matrix
Eigen::MatrixXd Winv = CalcPinv(W);
Eigen::MatrixXd temp = J*Winv*J.transpose();
Eigen::MatrixXd J_inverse = Winv*J.transpose()*CalcPinv(temp);
// Eigen::MatrixXd Winv = CalcPinv(W);
// Eigen::MatrixXd temp = J*Winv*J.transpose();
// Eigen::MatrixXd J_inverse = Winv*J.transpose()*CalcPinv(temp);
J_inverse = J.transpose();
return J_inverse;
}
......
......@@ -61,9 +61,10 @@ class CCommon_Fc
*/
template<typename _Matrix_Type_> static _Matrix_Type_ CalcPinv(const _Matrix_Type_ &a, double epsilon = std::numeric_limits<double>::epsilon())
{
Eigen::JacobiSVD< _Matrix_Type_ > svdd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
double tolerance = epsilon * std::max(a.cols(), a.rows()) *svdd.singularValues().array().abs()(0);
_Matrix_Type_ mpinv = svdd.matrixV() * (svdd.singularValues().array().abs() > tolerance).select(svdd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svdd.matrixU().adjoint();
//Eigen::JacobiSVD< _Matrix_Type_ > svdd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
//double tolerance = epsilon * std::max(a.cols(), a.rows()) *svdd.singularValues().array().abs()(0);
//_Matrix_Type_ mpinv = svdd.matrixV() * (svdd.singularValues().array().abs() > tolerance).select(svdd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svdd.matrixU().adjoint();
_Matrix_Type_ mpinv = a.transpose();
return mpinv;
}
......
......@@ -25,7 +25,8 @@ void CKinematics::UAMKine(const UAM::CQuad& quad, UAM::CArm& arm, UAM::CHT& HT,
Eigen::MatrixXd Jb2i = QuadJac(quad.pos);
// Inertial to body frames
Eigen::MatrixXd Ji2b = CCommon_Fc::CalcPinv(Jb2i);
// Eigen::MatrixXd Ji2b = CCommon_Fc::CalcPinv(Jb2i);
Eigen::MatrixXd Ji2b = Jb2i.inverse();
// Jacobian from quadrotor body to camera
Eigen::MatrixXd Jb2c = Eigen::MatrixXd::Zero(6,6);
......
......@@ -44,7 +44,8 @@ void CTaskIR::TaskErrorJac(const int& arm_dof, const double& inf_rad, const Eige
}
// }
Eigen::MatrixXd Hinv = UAM::CCommon_Fc::CalcPinv(H);
// Eigen::MatrixXd Hinv = UAM::CCommon_Fc::CalcPinv(H);
Eigen::MatrixXd Hinv = H.transpose();
// weighting matrix
Eigen::MatrixXd temp = JIR*Hinv*JIR.transpose();
......
......@@ -47,7 +47,8 @@ void CTaskJL::TaskErrorJac(UAM::CArm& arm, const Eigen::MatrixXd& jntlim_pos_des
JL.block(0,4,1,arm.nj) = pow_n*Jtemp.transpose();
// Task Jacobian Pseudo-inverse
JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL);
//JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL);
JL_pseudo = JL.transpose();
}
void CTaskJL::TaskErrorJacFull(UAM::CArm& arm, const Eigen::MatrixXd& jntlim_pos_des, Eigen::MatrixXd& jntlim_pos_error, Eigen::MatrixXd& sigmaL, Eigen::MatrixXd& JL, Eigen::MatrixXd& JL_pseudo)
......@@ -69,7 +70,8 @@ void CTaskJL::TaskErrorJacFull(UAM::CArm& arm, const Eigen::MatrixXd& jntlim_pos
}
// Task Jacobian Pseudo-inverse
JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL);
//JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL);
JL_pseudo = JL.transpose();
}
} // End of UAM namespace
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment