How to stop the pendulum movement?

Question

ryunosuke tazawa il 16 Lug 2021

0
Link

Link diretto a questa domanda

https://it.mathworks.com/matlabcentral/answers/879808-how-to-stop-the-pendulum-movement

Risposto: Sahaj il 10 Lug 2023

I am making program that pendulum throwing ball. But pendlum can not stop just one lap.

So I want to stop processing when the simple pendulum makes one lap.　Please help me.

clear all;
close all;
clc;
%% 各変数（単振り子）
b = 0.05;                                      % 減衰係数
g = 9.81;                                      % 重力加速度
l = 1.0;                                       % 振り子の長さ 
m = 1.0;                                       % 質量
Trq = 8;                                       % 乱数にしてaction
theta_0 = [0; 0];                            % 初期状態 2.25 45度
t_span = linspace(0,11,1500);                  % ステップ数sim size
%% 運動方程式を解く
[t, result] = ode45(@(t,theta) ode_func(t, theta, b, g,l,m,Trq),t_span,theta_0);
% result(i ,1)= theta, result(i,2)=w
theta =result(:,1);                           % 角度
w =  result(:,2);                             % 角速度
rad_theta = result(:,1)*180/pi;               % ラジアンに変換
%% 強化学習
RewardFunc = @(theta,w)(-(abs(theta)).^2 + -0.25*(abs(w)).^2);   % 報酬関数
learnRate = 0.99;                                                % 学習率
epsilon = 0.5;                                                   % ε
epsilonDecay = 0.98;                                          % εの減衰係数
discount = 0.9;                                               % 時間割引率　　　　
successRate = 1;                                              % 成功率
winBounds = 100;                        % 単振り子が目的の状態になった時の報酬 
startPt = [0; 0];                       % 振り子の初期位置，垂直下から開始
maxEpi = 2000;                          % 最大試行回数                                  
maxit = 15000;                          % 1試行の反復回数
substeps = 2;                           % 物理的なステップ数
dt = 0.05;                              % シミュレーション時間
%% トルク条件(Action)
Trq = randi([0 20],1,1);            % 0~5の整数を1行１列生成 トルクの値
actions = [-Trq, Trq];              % 与えるトルクの向き，右，左または0
%% 観測する状態 状態の更新
states = zeros(length(theta)*length(w),2);          % 状態 thetaとw
index = 1; 
for j=1:length(theta)                                                                                                                   
    for n = 1:length(w)                % states(theta,w)に変換                                   
         states(index,1)=theta(j);     % 角度をstatesに入れる
         states(index,2)=w(n);         % 角速度をstatesに入れる
         index=index+1;                                                 
     end
end
%% 報酬関数とQ関数にstates(theta,w)を入れる
R = RewardFunc(states(:,1),states(:,2));        % 報酬関数に角度，角速度を代入
Q = repmat(R,[1,3]);                            % Q関数に代入, Rを水平に3つ重ねる
%% 最大価値関数
V = zeros(size(states,1),1);                    % 価値関数の初期値
% 最大価値関数 length(x2)×length(x1)の配列に変更, Qの最大値,2はdim 
Voring = reshape(max(Q,[],2),[length(w),length(theta)]);                      
if theta > -1.25
    disp(theta);
end
k = 1;
for i=1:length(result(:,1))    
   %% 振り子の描画　ボールの距離の描画
    x0 = 0;
    y0 =0;
    x1 = 1*sin(result(i,1));
    y1 = -1*cos(result(i,1)); 
    subplot(1,2,2)
    ball_v0 = abs(result(i,2));
    ball_x0 = x1;
    ball_y0 = y1;
    ball_time = sqrt(2*abs(ball_y0)/g);
    ball_reach = x1 + ball_v0 * ball_time;
    if ball_reach > 0
        J = linspace(1,100);
        plot(k,ball_reach,'or','markersize',5,'MarkerFaceColor','r');
        xlim([0 1000])
        ylim([0 10])
        grid on
        hold on
    end
    
    
    subplot(1,2,1)
    plot([-1 1],[0 0],'linewidth',2,'color','k');
    hold on;
    line([x0 x1],[y0 y1],'color','b','linewidth',1.5);
    plot(x1, y1,'-o','markers',20,'markerfacecolor','m');
    grid on;
    title('Motion of Pendulum')
    axis([-2 2 -2 2])
    axis equal
    hold off
    M(k) = getframe(gcf);
    k = k+1;
    
    
end
movie(M)
videofile = VideoWriter('pendulum_motion.mp4', 'MPEG-4');
open(videofile)