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;
}
コード例 #2
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;


}