Eigen::MatrixXd left_cinv(const Eigen::MatrixXd Q, const Eigen::VectorXi TS, const Eigen::MatrixXd W, const double tol){
// ******************************************************************************************************
// Function that implements the continuous inverse as in:
// Mansard, N., et al.; A Unified Approach to Integrate Unilateral Constraints in the Stack of Tasks
// IEEE, Transactions on Robotics, 2009
//
// INPUT
// - Q: matrix from which we want to compute the continuous inverse
// - TS: task size vector with 'ts(i)' being the size of task 'i'. The sum of the elements of 'TS' == number of rows of 'Q'.
// - W: task activation matrix, a positive symmetric matrix with eigenvalues \in [0,1]
//
// OUTPUT
// - left_cinv = cinv(U' * Q, TS, S) * U'
// with:
// - H obtained from the SVD of W as W = U * S * U'
// - S: matrix of singular values of W
JacobiSVD<MatrixXd> svd(W, ComputeThinU | ComputeThinV);
// std::cout << "--- svd ---" << std::endl;
// std::cout << "svd.matrixU()" << std::endl << svd.matrixU() << std::endl;
// std::cout << "svd.matrixV()" << std::endl << svd.matrixV() << std::endl;
return cinv(svd.matrixU().transpose()*Q, TS, svd.singularValues(), tol) * svd.matrixU().transpose();
}
Eigen::MatrixXd cinv(const Eigen::MatrixXd M, const Eigen::VectorXi TS, const Eigen::MatrixXd H, const double tol){
// ******************************************************************************************************
// Function that implements the continuous inverse as in:
// Mansard, N., et al.; A Unified Approach to Integrate Unilateral Constraints in the Stack of Tasks
// IEEE, Transactions on Robotics, 2009
//
// INPUT
// - M: matrix from which we want to compute the continuous inverse
// - TS: task size vector with 'ts(i)' being the size of task 'i'. The sum of the elements of 'TS' == number of rows of 'M'.
// - H: task activation matrix whose diagonal elements \in [0,1]
//
// OUTPUT
// Given B(m) the set of all permutations without repetitions of the first 'm' elements,
// that is, B(m==3) = {{1},{2},{3},{1,2},{1,3},{2,3},{1,2,3}} then:
// - cinv = sum_{P \in B(m)}( prod_{i \in P}(h_i) * prod_{i !\in P}(1.0 - h_i) * pinv(M_P) )
// with:
// - m = TS.size()
// - M_P = H0_P * M and H0_P as the diagonal matrix with HO_P(j,j) = 1 if j \in P, otherwise HO_P(j,j) = 0.
Eigen::VectorXd vH(H.rows());
for (unsigned int i=0; i<vH.size(); ++i) vH(i) = H(i,i);
return cinv(M, TS, vH, tol);
}
void gaussj(complex_dble **a, int n,complex_dble **b,int m)
{
int *indxc,*indxr,*ipiv;
int i,icol,irow,j,k,l,ll;
double big;
complex_dble dum,pivinv,c;
indxc=ivector(1,n);
indxr=ivector(1,n);
ipiv=ivector(1,n);
for (j=1;j<=n;j++) ipiv[j]=0;
for (i=1;i<=n;i++) {
big=0.0;
for (j=1;j<=n;j++)
if (ipiv[j] != 1)
for (k=1;k<=n;k++) {
if (ipiv[k] == 0) {
if (cabs(a[j][k]) >= big) {
big=cabs(a[j][k]);
irow=j;
icol=k;
}
} else if (ipiv[k] > 1) nrerror("GAUSSJ: Singular Matrix-1");
}
++(ipiv[icol]);
if (irow != icol) {
for (l=1;l<=n;l++) SWAP(a[irow][l],a[icol][l])
for (l=1;l<=m;l++) SWAP(b[irow][l],b[icol][l])
}
indxr[i]=irow;
indxc[i]=icol;
if ((a[icol][icol].re == 0.0)&&(a[icol][icol].im == 0.0))
nrerror("GAUSSJ: Singular Matrix-2");
pivinv.re=cinv(a[icol][icol]);
a[icol][icol].re=1.0;
a[icol][icol].im=0.0;
for (l=1;l<=n;l++) a[icol][l] = cmul(a[icol][l],pivinv);
for (l=1;l<=m;l++) b[icol][l] = cmul(b[icol][l],pivinv);
for (ll=1;ll<=n;ll++)
if (ll != icol) {
dum.re=a[ll][icol].re;
dum.im=a[ll][icol].im;
a[ll][icol].re=0.0;
a[ll][icol].im=0.0;
for (l=1;l<=n;l++) {
c.re=a[ll][icol].re;
c.im=a[ll][icol].im;
a[ll][l].re = c.re-cmul(c,dum);
a[ll][l].im = c.im-cmul(c,dum);
}
for (l=1;l<=m;l++)
c.re=b[ll][icol].re;
c.im=b[ll][icol].im;
b[ll][l].re = c.re-cmul(c,dum);
b[ll][l].im = c.im-cmul(c,dum);
}
}
}
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;
}
Eigen::MatrixXd cinv(const Eigen::MatrixXd M, const Eigen::VectorXi TS, const Eigen::VectorXd H, const double tol){
// ******************************************************************************************************
// Function that implements the continuous inverse as in:
// Mansard, N., et al.; A Unified Approach to Integrate Unilateral Constraints in the Stack of Tasks
// IEEE, Transactions on Robotics, 2009
//
// INPUT
// - M: matrix from which we want to compute the continuous inverse
// - TS: task size vector with 'ts(i)' being the size of task 'i'. The sum of the elements of 'TS' == number of rows of 'M'.
// - H: task activation vector whose elements \in [0,1]
//
// OUTPUT
// Given B(m) the set of all permutations without repetitions of the first 'm' elements,
// that is, B(m==3) = {{1},{2},{3},{1,2},{1,3},{2,3},{1,2,3}} then:
// - cinv = sum_{P \in B(m)}( prod_{i \in P}(h_i) * prod_{i !\in P}(1.0 - h_i) * pinv(M_P) )
// with:
// - m = TS.size()
// - M_P = H0_P * M and H0_P as the diagonal matrix with HO_P(j,j) = 1 if j \in P, otherwise HO_P(j,j) = 0.
// std::cout << "M" << std::endl << M << std::endl;
// std::cout << "TS: " << TS.transpose() << std::endl;
// std::cout << "H: " << H.transpose() << std::endl;
unsigned int m_rows = M.rows();
unsigned int m_cols = M.cols();
// Gather task data
unsigned int n_tasks = TS.size();
std::vector< std::pair<unsigned int, unsigned int> > task_rows;
std::vector<Eigen::MatrixXd> task_M;
for (unsigned int i=0, curr_row=0; i<n_tasks; ++i){
task_rows.push_back( std::pair<unsigned int, unsigned int>(curr_row, curr_row+static_cast<unsigned int>(TS(i))) );
task_M.push_back(M.block(curr_row,0,TS(i),m_cols));
curr_row += static_cast<unsigned int>(TS(i));
}
// // Plot data by task
// std::cout << "curr rows" << std::endl;
// for (auto i=0; i<n_tasks; ++i){
// std::cout << task_rows[i].first << " " << task_rows[i].second << " - " << H[i] << std::endl;
// std::cout << "task_M_pinv[i]:" << std::endl << task_M[i] << std::endl;
// }
// Get number of active tasks
std::vector<unsigned int> active_tasks;
for (unsigned int i=0; i<n_tasks; ++i) if (H[i]) active_tasks.push_back(i);
unsigned int n_active_tasks = active_tasks.size();
// std::cout << "active_tasks: "; for (auto i=0; i<active_tasks.size(); ++i) std::cout << active_tasks[i] << " "; std::cout << std::endl;
Eigen::MatrixXd cinv(Eigen::MatrixXd::Zero(m_cols,m_rows));
for (unsigned int n_permutation_elem = 1; n_permutation_elem <= n_active_tasks; ++n_permutation_elem){
std::vector<bool> active_tasks_in_permutation(n_active_tasks);
std::fill(active_tasks_in_permutation.begin(), active_tasks_in_permutation.begin() + n_permutation_elem, true);
do {
Eigen::MatrixXd subset_task_m(Eigen::MatrixXd::Zero(m_rows,m_cols));
double h_subset_task_activation_prod = 1.0;
for (unsigned int j_task = 0; j_task < n_active_tasks; ++j_task) {
if (active_tasks_in_permutation[j_task]) {
subset_task_m.block(task_rows[active_tasks[j_task]].first,0,TS(active_tasks[j_task]),m_cols) = task_M[active_tasks[j_task]];
h_subset_task_activation_prod *= H(active_tasks[j_task]);
}
else
h_subset_task_activation_prod *= 1.0 - H(active_tasks[j_task]);
}
// std::cout << "permutation: "; for (auto k=0; k<active_tasks_in_permutation.size(); ++k) std::cout<< active_tasks_in_permutation[k] << " "; std::cout << std::endl;
// std::cout << "h_subset_task_activation_prod: " << h_subset_task_activation_prod << std::endl;
// std::cout << "subset_task_m" << std::endl << subset_task_m << std::endl;
cinv = cinv + h_subset_task_activation_prod * pinv(subset_task_m,tol);
} while (std::prev_permutation(active_tasks_in_permutation.begin(), active_tasks_in_permutation.end()));
}
return cinv;
}
void main() {
//* Initialize parameters (grid spacing, time step, etc.)
cout << "Enter number of grid points: "; int N; cin >> N;
double L = 100; // System extends from -L/2 to L/2
double h = L/(N-1); // Grid size
double h_bar = 1; double mass = 1; // Natural units
cout << "Enter time step: "; double tau; cin >> tau;
Matrix x(N);
int i, j, k;
for( i=1; i<=N; i++ )
x(i) = h*(i-1) - L/2; // Coordinates of grid points
//* Set up the Hamiltonian operator matrix
Matrix eye(N,N), ham(N,N);
eye.set(0.0); // Set all elements to zero
for( i=1; i<=N; i++ ) // Identity matrix
eye(i,i) = 1.0;
ham.set(0.0); // Set all elements to zero
double coeff = -h_bar*h_bar/(2*mass*h*h);
for( i=2; i<=(N-1); i++ ) {
ham(i,i-1) = coeff;
ham(i,i) = -2*coeff; // Set interior rows
ham(i,i+1) = coeff;
}
// First and last rows for periodic boundary conditions
ham(1,N) = coeff; ham(1,1) = -2*coeff; ham(1,2) = coeff;
ham(N,N-1) = coeff; ham(N,N) = -2*coeff; ham(N,1) = coeff;
//* Compute the Crank-Nicolson matrix
Matrix RealA(N,N), ImagA(N,N), RealB(N,N), ImagB(N,N);
for( i=1; i<=N; i++ )
for( j=1; j<=N; j++ ) {
RealA(i,j) = eye(i,j);
ImagA(i,j) = 0.5*tau/h_bar*ham(i,j);
RealB(i,j) = eye(i,j);
ImagB(i,j) = -0.5*tau/h_bar*ham(i,j);
}
Matrix RealAi(N,N), ImagAi(N,N);
cout << "Computing matrix inverse ... " << flush;
cinv( RealA, ImagA, RealAi, ImagAi ); // Complex matrix inverse
cout << "done" << endl;
Matrix RealD(N,N), ImagD(N,N); // Crank-Nicolson matrix
for( i=1; i<=N; i++ )
for( j=1; j<=N; j++ ) {
RealD(i,j) = 0.0; // Matrix (complex) multiplication
ImagD(i,j) = 0.0;
for( k=1; k<=N; k++ ) {
RealD(i,j) += RealAi(i,k)*RealB(k,j) - ImagAi(i,k)*ImagB(k,j);
ImagD(i,j) += RealAi(i,k)*ImagB(k,j) + ImagAi(i,k)*RealB(k,j);
}
}
//* Initialize the wavefunction
const double pi = 3.141592654;
double x0 = 0; // Location of the center of the wavepacket
double velocity = 0.5; // Average velocity of the packet
double k0 = mass*velocity/h_bar; // Average wavenumber
double sigma0 = L/10; // Standard deviation of the wavefunction
double Norm_psi = 1/(sqrt(sigma0*sqrt(pi))); // Normalization
Matrix RealPsi(N), ImagPsi(N), rpi(N), ipi(N);
for( i=1; i<=N; i++ ) {
double expFactor = exp(-(x(i)-x0)*(x(i)-x0)/(2*sigma0*sigma0));
RealPsi(i) = Norm_psi * cos(k0*x(i)) * expFactor;
ImagPsi(i) = Norm_psi * sin(k0*x(i)) * expFactor;
rpi(i) = RealPsi(i); // Record initial wavefunction
ipi(i) = ImagPsi(i); // for plotting
}
//* Initialize loop and plot variables
int nStep = (int)(L/(velocity*tau)); // Particle should circle system
int nplots = 20; // Number of plots to record
double plotStep = nStep/nplots; // Iterations between plots
Matrix p_plot(N,nplots+2);
for( i=1; i<=N; i++ ) // Record initial condition
p_plot(i,1) = RealPsi(i)*RealPsi(i) + ImagPsi(i)*ImagPsi(i);
int iplot = 1;
//* Loop over desired number of steps (wave circles system once)
int iStep;
Matrix RealNewPsi(N), ImagNewPsi(N);
for( iStep=1; iStep<=nStep; iStep++ ) {
//* Compute new wave function using the Crank-Nicolson scheme
RealNewPsi.set(0.0); ImagNewPsi.set(0.0);
for( i=1; i<=N; i++ ) // Matrix multiply D*psi
for( j=1; j<=N; j++ ) {
RealNewPsi(i) += RealD(i,j)*RealPsi(j) - ImagD(i,j)*ImagPsi(j);
ImagNewPsi(i) += RealD(i,j)*ImagPsi(j) + ImagD(i,j)*RealPsi(j);
}
RealPsi = RealNewPsi; // Copy new values into Psi
ImagPsi = ImagNewPsi;
//* Periodically record values for plotting
if( fmod(iStep,plotStep) < 1 ) {
iplot++;
for( i=1; i<=N; i++ )
p_plot(i,iplot) = RealPsi(i)*RealPsi(i) + ImagPsi(i)*ImagPsi(i);
cout << "Finished " << iStep << " of " << nStep << " steps" << endl;
}
}
// Record final probability density
iplot++;
for( i=1; i<=N; i++ )
p_plot(i,iplot) = RealPsi(i)*RealPsi(i) + ImagPsi(i)*ImagPsi(i);
nplots = iplot; // Actual number of plots recorded
//* Print out the plotting variables: x, rpi, ipi, p_plot
ofstream xOut("x.txt"), rpiOut("rpi.txt"), ipiOut("ipi.txt"),
p_plotOut("p_plot.txt");
for( i=1; i<=N; i++ ) {
xOut << x(i) << endl;
rpiOut << rpi(i) << endl;
ipiOut << ipi(i) << endl;
for( j=1; j<nplots; j++ )
p_plotOut << p_plot(i,j) << ", ";
p_plotOut << p_plot(i,nplots) << endl;
}
}
