使用 GSL 的双摆解决方案

Double pendulum solution using GSL

我正在学习使用 GSL 求解 ODE。我想使用 GSL ODE 函数解决双摆问题。我决定使用这个方程式:

(来源:http://www.physics.usyd.edu.au/~wheat/dpend_html/

我的代码:

#include <iostream>
#include <cmath>
#include "gsl/gsl_errno.h"
#include "gsl/gsl_matrix.h"
#include "gsl/gsl_odeiv2.h"
#include "constants.h"

double L1;
double L2;
double M1;
double M2;
double T_START;
double T_END;
double S1_ANGLE;
double S2_ANGLE;
double V1_INIT;
double V2_INIT;

int func(double t, const double y[], double f[], void *params) {

    /*
     * y[0] = theta_2
     * y[1] = omega_1
     * y[2] = theta_1
     * y[3] = omega_2
     */

    f[0] = y[1];
    double del = y[2] - y[1];
    double den1 = (M1 + M2) * L1 - M2 * L1 * cos(del) * cos(del);
    f[1] = (M2 * L1 * y[1] * y[1] * sin(del) * cos(del)
            + M2 * G * sin(y[2]) * cos(del) + M2 * L2 * y[3] * y[3] * sin(del)
            - (M1 + M2) * G * sin(y[0])) / den1;

    f[2] = y[3];
    double den2 = (L2 / L1) * den1;
    f[3] = (-M2 * L2 * y[3] * y[3] * sin(del) * cos(del)
            + (M1 + M2) * G * sin(y[0]) * cos(del)
            - (M1 + M2) * L1 * y[1] * y[1] * sin(del)
            - (M1 + M2) * G * sin(y[2])) / den2;

    return GSL_SUCCESS;
}


int main(int argc, char *argv[]) {

    /*
     * Arguments list:
     * 1 - length of pendulum 1
     * 2 - length of pendulum 2
     * 3 - mass of pendulum 1
     * 4 - mass of pendulum 2
     * 5 - start time (seconds)
     * 6 - end time (seconds)
     * 7 - initial angle of 1 pendulum (degrees)
     * 8 - initial angle od 2 pendulum
     * 9 - initial angular velocity of 1 pendulum (deegres per second)
     * 10 - initial angular velocity of 2 pendulum
     */

    if (argc != 11) {
        printf("Wrong number of arguments... \n");
        exit(1);
    }

    //Attribution of arguments
    L1 = atof(argv[1]);
    L2 = atof(argv[2]);
    M1 = atof(argv[3]);
    M2 = atof(argv[4]);
    T_START = atof(argv[5]);
    T_END = atof(argv[6]);
    S1_ANGLE = atof(argv[7]);
    S2_ANGLE = atof(argv[8]);
    V1_INIT = atof(argv[9]);
    V2_INIT = atof(argv[10]);

    //converting to radians
    S1_ANGLE=S1_ANGLE*PI/180.0;
    S2_ANGLE=S2_ANGLE*PI/180.0;
    V1_INIT=V1_INIT*PI/180.0;
    V2_INIT=V2_INIT*PI/180.0;
    printf("L1:%f\nL2: %f\nM1 :%f\nM2:%f\nT_START:%f\nT_END:%f\nS1_ANGLE: %f \nS2_ANGLE: %f\nV1_INIT: %f \nV2_INIT: %f \n",
    L1,L2,M1,M2,T_START,T_END,S1_ANGLE,S2_ANGLE,V1_INIT,V2_INIT);


    gsl_odeiv2_system sys = {func, NULL, 4, NULL};
    gsl_odeiv2_driver *d =
            gsl_odeiv2_driver_alloc_y_new(&sys, gsl_odeiv2_step_rk4, 1e-6, 1e-6, 0.0);



    double y[4] = {S2_ANGLE,V1_INIT,S1_ANGLE,V2_INIT};
    double t = T_START;
    for (int i = 1; i <= 100; i++) {
        double ti = i * (T_END - T_START) / 100.0;
        int status = gsl_odeiv2_driver_apply(d, &t, ti, y);
        printf("%.5e %.5e %.5e %.5e %.5e \n", t, y[0], y[1],y[2],y[3]);
    }


    return 0;
}

无论我输入什么参数,我总是得到一个错误:

gsl: driver.c:354: ERROR: integration limits and/or step direction not consistent Default GSL error handler invoked.

我不明白是什么导致了我的错误。

确实是y中参数的顺序。在引用的来源中,它是在 "natural" 顺序中,您混合的原因并不是很清楚。给一些子表达式命名,ODE 函数也可以写成

int func(double t, const double y[], double f[], void *params) {


    double th1 = y[0], w1 = y[1];
    double th2 = y[2], w2 = y[3];

    f[0] = w1; // dot theta_1 = omega_1
    f[2] = w2; // dot theta_2 = omega_2

    double del = th2 - th1;
    double den = (M1 + M2) - M2 * cos(del) * cos(del);
    double Lwws1 = L1 * (w1*w1) * sin(del);
    double Lwws2 = L2 * (w2*w2) * sin(del);
    double Gs1 = G*sin(th1), Gs2 = G*sin(th2);

    f[1] = (M2 * (Lwws1 + Gs2) * cos(del) + M2 * Lwws2 - (M1 + M2) * Gs1) / (L1*den);

    f[3] = (-M2 * Lwws2 * cos(del) + (M1 + M2) * ( Gs1 * cos(del) - Lwws1 - Gs2) / (L2*den);

    return GSL_SUCCESS;
}

当然,这里假设初始点向量定义为

    double y[4] = {S1_ANGLE,V1_INIT,S2_ANGLE,V2_INIT};

结果的解释与此相对应。

您也可以将 C++ 与 odeint library if you are interested. For the double pendulum system. For matrices I use Eigen and for solving ODEs I use odeint 一起使用,因此这是解决您的问题的代码。

#include <iostream>
#include <cmath>
#include <Eigen/Dense>
#include <boost/math/constants/constants.hpp>
#include <boost/numeric/odeint.hpp>
#include <iomanip>

using namespace std;
using namespace boost::numeric::odeint;


typedef std::vector< double > state_type;


void equations(const state_type &y, state_type &dy, double x)
{
    const double m1(0.5), m2(0.5),
                 L1(0.1), L2(0.1),
                 g(9.81);

    Eigen::MatrixXd M(2, 2), C(2, 1), B(2,1);

    /*
      Theta 1 =  y[0]
     dTheta 1 =  y[1] = dy[0]
    ddTheta 1 = dy[1]

      Theta 2 =  y[2]
     dTheta 2 =  y[3] = dy[2]
    ddTheta 2 = dy[3]
    */
    double delta(y[0] - y[2]);

    M <<      (m1 + m2)*L1, m2*L2*cos(delta),
          m2*L1*cos(delta),            m2*L2;


    C <<  m2*L1*L2*y[3]*y[3]*sin(delta) + g*(m1 + m2)*sin(y[0]),
         -m2*L1*y[1]*y[1]*sin(delta)    +        m2*g*sin(y[2]);


    //#####################( ODE Equations )################################
    dy[0] = y[1];
    dy[2] = y[3];

    B = M.inverse() * (-C);


    dy[1] = B(0,0);
    dy[3] = B(1,0);

}


int main(int argc, char **argv)
{
    const double dt = 0.01;
    runge_kutta_dopri5 < state_type > stepper;
    double pi = boost::math::constants::pi<double>();

    state_type y(4); 
    // initial values
    y[0] = pi/3.0;  //  Theta 1 
    y[1] = 0.0;     // dTheta 1
    y[2] = pi/4.0; //   Theta 2
    y[3] = 0.0;    //  dTheta 2

    for (double t(0.0); t <= 5; t += dt){

        cout << fixed << setprecision(2);
        std::cout << "t: " << t << "  Theta 1: " << y[0] << "   dTheta 1: " << y[1]
                  << "  Theta 2: " << y[2] << "  dTheta 2: " << y[3] << std::endl;


        stepper.do_step(equations, y, t, dt);
    }

}

结果是