int test()
{
time_t t;
t = time(NULL);
srand(unsigned(t));
Matrix3 R0;
Vector3 Vrl0, axis0, ori, theta0;
AngleAxis j0;
/// The number of samples
const unsigned kmax=2000;
///sampling period
const double dt=5e-3;
///Sizes of the states for the state, the measurement, and the input vector
const unsigned stateSize=18;
const unsigned measurementSize=6;
const unsigned inputSize=48;
(void)measurementSize;
///The array containing all the states, the measurements and the inputs
IndexedVectorArray x;
IndexedVectorArray u;
///simulation of the signal
/// the IMU dynamical system functor
flexibilityEstimation::IMUElasticLocalFrameDynamicalSystem imu(dt);
imu.setInputSize(inputSize);
///the simulator initalization
DynamicalSystemSimulator sim;
sim.setDynamicsFunctor(& imu); // choice of the dynamical system
//std::cout << "hello" << std::endl;
/// Input initialization
Vector6 Inert;
Vector uk=Vector::Zero(imu.getInputSize(),1);
// // Inertia, the robot is considered as a full cylindar
// Inert << 0.25*hrp2::m*hrp2::R*hrp2::R+0.33*hrp2::m*hrp2::H*hrp2::H, 0.25*hrp2::m*hrp2::R*hrp2::R+0.33*hrp2::m*hrp2::H*hrp2::H, 0.5*hrp2::m*hrp2::R*hrp2::R, 0.0, 0.0, 0.0;
// uk.segment(input::Inertia,6)=Inert;
//
// // Contacts
// imu.setContactsNumber(1);
// uk.segment(input::contacts,6) << 0.0094904630937003645, -0.095000000000000001, 1.9819700013135044e-07,0,0,0;
// uk.segment(input::contacts+6,6) << 0.0094904630936998632, 0.095000000000000001, 1.9819700018686159e-07,0,0,0;
//
// // Input init
// uk.segment(input::posCom,3) << 0,
// 0,
// 0.80771;//hrp2::H*0.5;
//
// uk.segment(input::accCom,3) << 0,
// 0,
// -9.8;
uk << 0.0135672,
0.001536,
0.80771,
-2.50425e-06,
-1.03787e-08,
5.4317e-08,
-2.50434e-06,
-1.03944e-08,
5.45321e-08,
48.1348,
46.9498,
1.76068,
-0.0863332,
-0.59487,
-0.0402246,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
-0.098,
-1.21619e-10,
1.1174,
3.06752e-22,
-1.06094e-20,
7.75345e-22,
-2.84609e-06,
-1.18496e-08,
-4.52691e-18,
2.95535e-20,
-1.0346e-18,
7.58731e-20,
-0.000284609,
-1.18496e-06,
-4.52691e-16,
0.0094904630937003645,
0,//-0.15000000000000001,
1.9819700013135044e-07,
0,
0,
0;
// 0.0094904630936998632,
// 0.15000000000000001,
// 1.9819700018686159e-07,
// 0,
// 0,
// 0;
// // contacts position
// double angleZ;
// angleZ=-PI/4;
//
// Vector6 v1, v2;
// v1 << -0.095*tan(angleZ),//0.0094904630936998632,
// 0.095000000000000001,
// 1.9819700018686159e-07,
// 0,
// 0,
// angleZ;
//
// v2 << 0.095*tan(angleZ),//0.0094904630936998632+
// -0.095000000000000001,
// 1.9819700018686159e-07,
// 0,
// 0,
// angleZ;
//
// Matrix4 cont1, cont2;
// cont1=kine::vector6ToHomogeneousMatrix(v1);
// cont2=kine::vector6ToHomogeneousMatrix(v2);
// uk.segment(input::contacts,6) << cont1(0,3), cont1(1,3), cont1(2,3), cont1(1,2), cont1(0,2), cont1(0,1);//0.0094904630937003645, -0.095000000000000001, 1.9819700013135044e-07,0,0,0;
// uk.segment(input::contacts+6,6) << cont2(0,3), cont2(1,3), cont2(2,3), cont2(1,2), cont2(0,2), cont2(0,1);//0.0094904630936998632, 0.095000000000000001, 1.9819700018686159e-07,0,0,0;
// std::cout << uk.transpose() << std::endl;
// for (i=0;i<uk.size();++i)
// {
// if(uk(i)<0.001 && uk(i)>-0.001)
// {
// uk(i)=0.0;
// }
// }
/// State initialization
Vector x0=(Vector::Zero(stateSize,1));
x0.segment(indexes::pos,3) << 0,
0,
0;//-9.8*hrp2::m / (2*hrp2::linKe); // flexibility excited by the weight
// To put a well interpretable excitation despite of a rotated axe for the abnkles.
std::cout << "hello" << std::endl;
// ori << 0;//-2*PI/180,
// 0;//2*PI/180,
// 0;
// h=rotationMatrixFromContactsPositionKine(uk.segment(input::contacts,6),uk.segment(input::contacts+6,6),R0);
// x0.segment(kine::ori,3) << R0.transpose()*ori;
// x0.segment(kine::ori,3) << ori;
// for (i=0;i<x0.size();++i)
// {
// if(x0(i)<0.1 && x0(i)>-0.1)
// {
// x0(i)=0.0;
// }
// }
sim.setState(x0,0);
for (int k=0;k<10;++k)
{
u.setValue(uk,k);
///give the input to the simulator
///we only need to give one value and the
///simulator takes automatically the appropriate value
sim.setInput(uk,k);
}
///set the sampling perdiod to the functor
imu.setSamplingPeriod(dt);
sim.simulateDynamicsTo(kmax);
x = sim.getStateArray(1,kmax);
x.writeInFile("state.dat");
u.writeInFile("input.dat");
//std::cout <<x[kmax].norm ()<< " "<< x[kmax].transpose() << std::endl;
//imu.test(); // test unitaires
return 0;
}
int testDerivator()
{
/// The number of samples
const unsigned kmax=3000;
///sampling period
const double dt=1e-3;
///Sizes of the states for the state, the measurement, and the input vector
const unsigned stateSize=18;
const unsigned measurementSize=6;
//const unsigned inputSize=6;
///The array containing all the states, the measurements and the inputs
IndexedMatrixArray x;
IndexedMatrixArray y;
IndexedMatrixArray u;
///The covariance matrix of the process noise and the measurement noise
Matrix q;
Matrix r;
{
///simulation of the signal
/// the IMU dynamical system functor
IMUDynamicalSystem imu;
///The process noise initialization
Matrix q1=Matrix::Identity(stateSize,stateSize)*0.00;
GaussianWhiteNoise processNoise(imu.getStateSize());
processNoise.setStandardDeviation(q1);
imu.setProcessNoise( & processNoise );
q=q1*q1.transpose();
///The measurement noise initialization
Matrix r1=Matrix::Identity(measurementSize,measurementSize)*0.0;
GaussianWhiteNoise MeasurementNoise(imu.getMeasurementSize());
MeasurementNoise.setStandardDeviation(r1);
imu.setMeasurementNoise( & MeasurementNoise );
r=r1*r1.transpose();
///the simulator initalization
DynamicalSystemSimulator sim;
sim.setDynamicsFunctor(&imu);
///initialization of the state vector
Vector x0=Vector::Zero(stateSize,1);
sim.setState(x0,0);
///construction of the input
/// the input is constant over 10 time samples
for (unsigned i=0;i<kmax/10;++i)
{
Vector uk=Vector::Zero(imu.getInputSize(),1);
uk[0]=0.4 * sin(M_PI/10*i);
uk[1]=0.6 * sin(M_PI/12*i);
uk[2]=0.2 * sin(M_PI/5*i);
uk[3]=10 * sin(M_PI/12*i);
uk[4]=0.07 * sin(M_PI/15*i);
uk[5]=0.05 * sin(M_PI/5*i);
///filling the 10 time samples of the constant input
for (int j=0;j<10;++j)
{
u.setValue(uk,i*10+j);
}
///give the input to the simulator
///we only need to give one value and the
///simulator takes automatically the appropriate value
sim.setInput(uk,10*i);
}
///set the sampling perdiod to the functor
imu.setSamplingPeriod(dt);
///launched the simulation to the time kmax+1
sim.simulateDynamicsTo(kmax+1);
///extract the array of measurements and states
y = sim.getMeasurementArray(1,kmax);
x = sim.getStateArray(1,kmax);
}
IndexedMatrixArray dta;
for (unsigned i=x.getFirstIndex() ; i<=x.getLastIndex() ; ++i)
{
Vector xi = Vector::Zero(6,1);
xi.head(3) = Vector(x[i]).segment(kine::pos,3);
xi.tail(3) = Vector(x[i]).segment(kine::ori,3);
dta.setValue(xi,i);
}
IndexedMatrixArray state = kine::reconstructStateTrajectory(dta,dt);
///file of output
std::ofstream f;
f.open("trajectory.dat");
for (unsigned i=x.getFirstIndex() ; i<=x.getLastIndex() ; ++i)
{
//f<<dta[i].transpose()<<"\t#####\t\t"<<Vector(Vector(x[i]).segment(9,3)).transpose()<<"\t#####\t\t"<<(x[i]-state[i]).transpose()<<std::endl;
f<<(x[i]-state[i])<<std::endl<<std::endl;
}
return 0;
}
