How to stop the pendulum movement?

1 view (last 30 days)
ryunosuke tazawa
ryunosuke tazawa on 16 Jul 2021
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行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)

Answers (0)

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by