IqWampSubscription::IqWampSubscription(int id, const QString &topic) :
<<<<<<< HEAD
m_topic(topic),
m_id(id)
=======
m_id(id),
m_topic(topic)
>>>>>>> 47d4c027f824f401bb90eb267e7b023187f9e899
{
}
QString IqWampSubscription::topic() const
{
return m_topic;
}
int IqWampSubscription::id() const
{
return m_id;
}
int main(void){
std::cout << " ---- Programa de Tests de la pseudoinverse de Mansard ----" << std::endl;
unsigned int nq = 10;
unsigned int nx1 = 7;
Eigen::MatrixXd H1(Eigen::MatrixXd::Zero(nx1,nx1));
H1(0,0) = 0;
H1(1,1) = 0.2;
H1(2,2) = 0;
H1(3,3) = 0.5;
H1(4,4) = 0;
H1(5,5) = 0;
H1(6,6) = 0.7;
std::vector<unsigned int> active_joints;
for (unsigned int i=0; i<nx1; ++i) if (H1(i,i)) active_joints.push_back(i);
unsigned int n_active_joints = active_joints.size();
std::cout << "n_active_joints: " << n_active_joints << std::endl;
// Initialize pseudoinverse to zero
Eigen::MatrixXd pJ1(Eigen::MatrixXd::Zero(nq,nx1));
// // Per cada grup de 'i' elements
// for (unsigned int i=1; i<=n_active_joints; ++i){
// std::vector<bool> v(n_active_joints);
// std::fill(v.begin(), v.begin() + i, true);
// do {
// Eigen::MatrixXd J_sum(Eigen::MatrixXd::Zero(nx1,nq));
// double h_prod = 1.0;
// for (int j = 0; j < n_active_joints; ++j) {
// unsigned int qi = active_joints[j];
// if (v[j]) {
//// std::cout << j << " ";
// std::cout << qi << " ";
// J_sum(qi,qi+3) = 1.0;
// h_prod *= H1(qi,qi);
// }
// else h_prod *= 1.0 - H1(qi,qi);
// }
// std::cout << "\n";
//// std::cout << " - P h: " << h_prod << "\n";
// std::cout << " J_sum: " << "\n" << J_sum << "\n";
// Eigen::MatrixXd pJ_sum(pinv(J_sum,0.00001));
// std::cout << " h_prod * pJ_sum: " << "\n" << h_prod * pJ_sum << "\n";
// pJ1 = pJ1 + h_prod * pJ_sum;
// std::cout << " pJ1: " << "\n" << pJ1 << "\n";
// } while (std::prev_permutation(v.begin(), v.end()));
// std::cout << "-----\n";
// }
// std::cout << " pJ1: " << "\n" << pJ1 << "\n";
// Per cada grup de 'i' elements
pJ1 = Eigen::MatrixXd::Zero(nq,nx1);
for (unsigned int i=1; i<=n_active_joints; ++i){
std::vector<bool> v(n_active_joints);
std::fill(v.begin(), v.begin() + i, true);
do {
Eigen::MatrixXd J_sum(Eigen::MatrixXd::Zero(nx1,nq));
double h_prod = 1.0;
for (unsigned int j = 0; j < n_active_joints; ++j) {
unsigned int qi = active_joints[j];
if (v[j]) {
J_sum(qi,qi+3) = 1.0;
h_prod *= H1(qi,qi);
}
else
h_prod *= 1.0 - H1(qi,qi);
}
pJ1 = pJ1 + h_prod * pinv(J_sum,0.00001);
} while (std::prev_permutation(v.begin(), v.end()));
}
std::cout << " pJ1: " << "\n" << pJ1 << "\n";
// Eigen::MatrixXd m_id(Eigen::MatrixXd::Identity(6,6));
// m_id(1,1) = 0.2;
// m_id(4,4) = -3;
// Eigen::VectorXi TS(3);
// TS(0) = 2;
// TS(1) = 3;
// TS(2) = 1;
// Eigen::VectorXd H(3);
// H(0) = 0.1;
// H(1) = 0.6;
// H(2) = 0.4;
// std::cout << " cinv: " << "\n" << cinv(m_id,TS,H) << "\n";
double tol = 0.0000001;
Eigen::MatrixXd m_id(Eigen::MatrixXd::Identity(nx1,nx1));
Eigen::VectorXi TS(nx1);
for (unsigned int i=0; i<nx1; ++i) TS(i) = 1.0;
Eigen::VectorXd H(nx1);
H(0) = 0;
H(1) = 0.2;
H(2) = 0;
H(3) = 0.5;
H(4) = 0;
H(5) = 0;
H(6) = 0.7;
std::cout << "cinv: " << "\n" << cinv(m_id,TS,H) << "\n";
Eigen::MatrixXd lH(nx1,nx1);
lH(0,0) = 0;
lH(1,1) = 0.2;
lH(2,2) = 0;
lH(3,3) = 0.5;
lH(4,4) = 0;
lH(5,5) = 0;
lH(6,6) = 0.7;
std::cout << "cinv: " << "\n" << cinv(m_id,TS,lH) << "\n";
std::cout << "left cinv: " << "\n" << left_cinv(m_id,TS,lH) << "\n";
std::cout << "right cinv: " << "\n" << right_cinv(m_id,TS,lH) << "\n";
return 0;
}
