使用GSL的雙擺解決方案

Question

我正在學習使用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; 
} 

這真的並不重要參數I輸入，我總是得到一個錯誤：

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

我不明白是什麼原因導致我的錯誤。

Answer 1

這實際上是y中的參數順序。在引用的來源中，它是按照「自然」順序排列的，您混音的原因並不十分清楚。給予名稱一些子表達式時，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}; 

和結果的解釋對應於此。

Answer 2

如果您有興趣，也可以使用C++與odeint庫。對於雙擺系統。對於矩陣我使用Eigen和解決ODE我使用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); 
    } 

} 

的結果是