/// Main program of uncertainty propagation of the ODE model parameters via intrusive spectral projection (ISP)
int main()
{
// Model parameters
Array1D<double> modelparams;
// Model parameter names
Array1D<string> modelparamnames;
// Auxiliary parameters: final time and time step of integration
Array1D<double> modelauxparams;
// Read the xml tree
RefPtr<XMLElement> xmlTree=readXMLTree("lorenz.in.xml");
// Read the model-specific input
readXMLModelInput(xmlTree,modelparams, modelparamnames, modelauxparams);
// Total nuber of input parameters
int fulldim=modelparams.XSize();
// Read the output preferences
dumpInfo* outPrint=new dumpInfo;
readXMLDumpInfo( xmlTree, &(outPrint->dumpInt), &(outPrint->fdumpInt), &(outPrint->dumpfile) );
// Output PC order
int order;
// PC type
string pcType;
// A 2d array (each row is an array of coefficients for the corresponding uncertain input parameter)
Array2D<double> allPCcoefs;
// The indices of the uncertain model parameters in the list of model parameters
Array1D<int> uncParamInd;
// Read the UQ-specific information from the xml tree
readXMLUncInput(xmlTree,allPCcoefs,uncParamInd , &order, &pcType);
// Stochastic dimensionality
int dim=uncParamInd.XSize();
// Instantiate a PC object for ISP computations
PCSet myPCSet("ISP",order,dim,pcType,0.0,1.0);
// The number of PC terms
const int nPCTerms = myPCSet.GetNumberPCTerms();
cout << "The number of PC terms in an expansion is " << nPCTerms << endl;
// Print the multiindices on screen
myPCSet.PrintMultiIndex();
// Initial time
double t0 = 0.0;
// Final time
double tf = modelauxparams(0);
// Time step
double dTym = modelauxparams(1);
// Number of steps
int nStep=(int) tf / dTym;
// Initial conditions of zero coverage (based on Makeev:2002)
Array1D<double> u(nPCTerms,0.e0);
Array1D<double> v(nPCTerms,0.e0);
Array1D<double> w(nPCTerms,0.e0);
Array1D<double> z(nPCTerms,0.e0);
// Array to hold the PC representation of the number 1
Array1D<double> one(nPCTerms,0.e0);
one(0)=1.0;
// The z-species is described as z=1-u-v-w
z=one;
myPCSet.SubtractInPlace(z,u);
myPCSet.SubtractInPlace(z,v);
myPCSet.SubtractInPlace(z,w);
// Right-hand sides
Array1D<double> dudt(nPCTerms,0.e0);
Array1D<double> dvdt(nPCTerms,0.e0);
Array1D<double> dwdt(nPCTerms,0.e0);
// Array of arrays to hold the input parameter PC representations in the output PC
// Each element is an array of coefficients for the corresponding input parameter, whether deterministic or uncertain
// The size of the array is the total number input parameters
Array1D< Array1D<double> > modelparamPCs(fulldim);
printf("\nInput parameter PC coefficients are given below\n");
for (int i=0; i<fulldim; i++){
printf("%s: ",modelparamnames(i).c_str());
modelparamPCs(i).Resize(nPCTerms,0.e0);
for (int j=0; j<nPCTerms; j++){
modelparamPCs(i)(j)=allPCcoefs(j,i);
printf(" %lg ",modelparamPCs(i)(j));
}
printf("\n");
}
printf("\n");
// Initial time and time step counter
int step=0;
double tym=t0;
// Work arrays for integration
Array1D<double> u_o(nPCTerms,0.e0);
Array1D<double> v_o(nPCTerms,0.e0);
Array1D<double> w_o(nPCTerms,0.e0);
Array1D<double> tmp_u(nPCTerms,0.e0);
Array1D<double> tmp_v(nPCTerms,0.e0);
Array1D<double> tmp_w(nPCTerms,0.e0);
// File to write the mean and stdev, name read from xml
FILE *f_dump,*modes_dump;
if(!(f_dump = fopen(outPrint->dumpfile.c_str(),"w"))){
printf("Could not open file '%s'\n",outPrint->dumpfile.c_str());
exit(1);
}
// File to dump the PC modes, name hardwired
string modes_dumpfile = "solution_ISP_modes.dat";
if(!(modes_dump = fopen(modes_dumpfile.c_str(),"w"))){
printf("Could not open file '%s'\n",modes_dumpfile.c_str());
exit(1);
}
// write time, u, v, w (all modes) to file
WriteModesToFilePtr(tym, u.GetArrayPointer(), v.GetArrayPointer(), w.GetArrayPointer(), nPCTerms, modes_dump);
// Write out initial step
// Get standard deviations
double uStDv = myPCSet.StDv(u);
double vStDv = myPCSet.StDv(v);
double wStDv = myPCSet.StDv(w);
// write u, v, w (mean and standard deviation) to file
WriteMeanStdDevToFilePtr(tym, u(0), v(0), w(0), uStDv, vStDv, wStDv, f_dump);
// write u, v, w (mean and standard deviation) to screen
WriteMeanStdDevToStdOut(step, tym, u(0), v(0), w(0), uStDv, vStDv, wStDv);
// Forward run
while(tym < tf) {
// Integrate with 2nd order Runge Kutta
// Save solution at current time step
myPCSet.Copy(u_o,u);
myPCSet.Copy(v_o,v);
myPCSet.Copy(w_o,w);
// Compute right hand sides
GetRHS(myPCSet,modelparamPCs(0).GetArrayPointer(),modelparamPCs(1).GetArrayPointer(),modelparamPCs(2).GetArrayPointer(),u.GetArrayPointer(),v.GetArrayPointer(),w.GetArrayPointer(),dudt.GetArrayPointer(),dvdt.GetArrayPointer(),dwdt.GetArrayPointer());
// Advance u, v, w to mid-point
myPCSet.Multiply(dudt,0.5*dTym,tmp_u); // 0.5*dTym*dudt
myPCSet.Multiply(dvdt,0.5*dTym,tmp_v); // 0.5*dTym*dvdt
myPCSet.Multiply(dwdt,0.5*dTym,tmp_w); // 0.5*dTym*dwdt
myPCSet.Add(u_o,tmp_u,u); // u = u_o + 0.5*dTym*dudt
myPCSet.Add(v_o,tmp_v,v); // v = v_o + 0.5*dTym*dvdt
myPCSet.Add(w_o,tmp_w,w); // w = w_o + 0.5*dTym*dwdt
// Compute z = 1 - u - v - w
z=one;
myPCSet.SubtractInPlace(z,u);
myPCSet.SubtractInPlace(z,v);
myPCSet.SubtractInPlace(z,w);
// Compute right hand sides
GetRHS(myPCSet,modelparamPCs(0).GetArrayPointer(),modelparamPCs(1).GetArrayPointer(),modelparamPCs(2).GetArrayPointer(),u.GetArrayPointer(),v.GetArrayPointer(),w.GetArrayPointer(),dudt.GetArrayPointer(),dvdt.GetArrayPointer(),dwdt.GetArrayPointer());
// Advance u, v, w to next time step
myPCSet.Multiply(dudt,dTym,tmp_u); // dTym*dudt
myPCSet.Multiply(dvdt,dTym,tmp_v); // dTym*dvdt
myPCSet.Multiply(dwdt,dTym,tmp_w); // dTym*dwdt
myPCSet.Add(u_o,tmp_u,u); // u = u_o + dTym*dudt
myPCSet.Add(v_o,tmp_v,v); // v = v_o + dTym*dvdt
myPCSet.Add(w_o,tmp_w,w); // w = w_o + dTym*dwdt
// Compute z = 1 - u - v - w
z=one;
myPCSet.SubtractInPlace(z,u);
myPCSet.SubtractInPlace(z,v);
myPCSet.SubtractInPlace(z,w);
// Advance time and step counter
tym += dTym;
step+=1;
// write time, u, v, w (all modes) to file
if(step % outPrint->fdumpInt == 0){
WriteModesToFilePtr(tym, u.GetArrayPointer(), v.GetArrayPointer(), w.GetArrayPointer(), nPCTerms, modes_dump);
}
// Get standard deviations
uStDv = myPCSet.StDv(u);
vStDv = myPCSet.StDv(v);
wStDv = myPCSet.StDv(w);
// write u, v, w (mean and standard deviation) to file
if(step % outPrint->fdumpInt == 0){
WriteMeanStdDevToFilePtr(tym, u(0), v(0), w(0), uStDv, vStDv, wStDv, f_dump);
}
// write u, v, w (mean and standard deviation) to screen
if(step % outPrint->dumpInt == 0){
WriteMeanStdDevToStdOut(step, tym, u(0), v(0), w(0), uStDv, vStDv, wStDv);
}
}
// Close output file
if(fclose(f_dump)){
printf("Could not close file '%s'\n",outPrint->dumpfile.c_str());
exit(1);
}
// Close output file
if(fclose(modes_dump)){
printf("Could not close file '%s'\n",modes_dumpfile.c_str());
exit(1);
}
return 0;
}
Eigen::VectorXd controlEnv::mobile_manip_inverse_kinematics_siciliano(void){
// Siciliano; modelling, planning and control; pag 139
// --------------------------------------------------------------------------------
// std::cout << "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA: " << n_iteration_ << std::endl;
// Position
VectorXd p_err(3);
for (unsigned int i=0; i<3; i++) p_err(i) = pose_error_.p()(i);
MatrixXd Kp(3,3);
Kp = MatrixXd::Zero(3,3);
Kp(0,0) = 1.0;
Kp(1,1) = 1.0;
Kp(2,2) = 1.0;
VectorXd vd(3);
desired_pose_velocity_ = compute_pose_velocity(desired_pose_, past_desired_pose_, time_increment_);
vd = desired_pose_velocity_.p();
VectorXd v_p(3);
v_p = vd + Kp * p_err;
// Orientation
Quaternion<double> rot_current, rot_desired;
rot_current = mobile_manip_.tcp_pose().o();
rot_desired = desired_pose_.o();
MatrixXd L(3,3);
L = Lmat(rot_current, rot_desired);
MatrixXd Sne(3,3), Sse(3,3), Sae(3,3);
Sne = cross_product_matrix_S(rot_current.toRotationMatrix().col(0));
Sse = cross_product_matrix_S(rot_current.toRotationMatrix().col(1));
Sae = cross_product_matrix_S(rot_current.toRotationMatrix().col(2));
VectorXd nd(3), sd(3), ad(3);
nd = rot_desired.toRotationMatrix().col(0);
sd = rot_desired.toRotationMatrix().col(1);
ad = rot_desired.toRotationMatrix().col(2);
VectorXd o_err(3);
o_err = 0.5 * ( Sne*nd + Sse*sd + Sae*ad );
MatrixXd Ko(3,3);
Ko = MatrixXd::Zero(3,3);
Ko(0,0) = 1.0;
Ko(1,1) = 1.0;
Ko(2,2) = 1.0;
VectorXd wd(3);
wd = desired_pose_velocity_.o_angaxis_r3();
VectorXd v_o(3);
v_o = L.inverse() * (L.transpose() * wd + Ko * o_err );
// Put all together
VectorXd v(6);
for (unsigned int i=0; i<3; i++){
v(i) = v_p(i);
v(i+3) = v_o(i);
}
return pinv(jacobian(mobile_manip_), 0.001)*v;
}
