Gazebo(Ros2)上で倒立振子を倒立させたい。

実現したいこと

Gazebo(Ros2)上で倒立振子を倒立させたいです。
LQRを使って制御入力を決めて、ガゼボ上の３Dモデルを倒立させたいと考えています。
gazeboへの入力はeffort controllerを使って、台車に左右方向に力を与える設計としたいです。

発生している問題・分からないこと

倒立のための挙動はしているように見えますが、時間経過ののちいずれ転倒してしまいます。
また、一般化座標を[x, x_dot, θ, θ_dot]T(x:台車変位/x_dot:台車速度/θ:角度/θ_dot:角速度)としています。
LQRの台車速度と角速度のパラメータを大きくすると、速度の目標は０なので、移動がゆっくりになる理解をしていましたが、シミュレーションだと逆に早く移動しているように見えます。
運動方程式等、誤りがないかは確認したつもりですが、原因がわからずヒントを頂けると幸いです。
プログラミングも初心者で、chatGPTに聞きながら進めているので、不足している情報等あればご指摘いただけると幸いです。

該当のソースコード

controllerのscript
1#!/usr/bin/env python3
2# -*- coding: utf-8 -*-
3import rclpy
4from rclpy.node import Node
5from std_msgs.msg import Float64MultiArray
6from sensor_msgs.msg import JointState
7import math  # sin関数の計算に必要
8from std_msgs.msg import Float64MultiArray
9import numpy as np
10import control as ctl
11
12class EffortControllerWithFeedback(Node):
13    def __init__(self):
14        super().__init__('effort_controller_with_feedback')
15        self.effort_publisher = self.create_publisher(Float64MultiArray, '/effort_controller/commands', 10)
16        self.joint_state_subscription = self.create_subscription(JointState, '/joint_states', self.joint_state_callback, 10)
17
18        self.init_time = self.get_clock().now()  # 初期化時の時刻を記録
19        self.angle_threshold_exceeded = False
20        
21        # 物理パラメータ
22        m = 2.4048913121650237  # 振り子の質量 (kg)
23        M = 0.11661244128735532  # 台車の質量 (kg)
24        l = 0.19588623318570866  # 振り子の長さ (m)
25        g = 9.81  # 重力加速度 (m/s^2)
26        
27        # 状態行列 (A) と入力行列 (B) の計算
28        self.A = np.array([[0, 1, 0, 0],
29                           [0, 0, -m*g/M, 0],
30                           [0, 0, 0, 1],
31                           [0, 0, g*(1+m/M)/l, 0]])
32        self.B = np.array([[0], [1/M], [0], [-1/(M*l)]])
33        
34        ## LQR制御器のパラメータ
35        self.Q = np.diag([20, 5, 0.1, 0.1])
36        self.R = np.array([[3.14]])
37        self.K, _, _ = ctl.lqr(self.A, self.B, self.Q, self.R)
38        
39        # 初期状態
40        self.state = np.array([[0], [0], [0.0], [0]])  # [x, x_dot, theta, theta_dot]
41
42    def joint_state_callback(self, msg):
43        # ジョイントの名前と一致する位置情報を検索
44        pendulum_joint_name = 'slider_to_pendulum_joint'
45        slider_joint_name = 'rail_to_slider_joint'
46        pendulum_position = slider_position = pendulum_velocity = slider_velocity = None
47
48        for name, position, velocity in zip(msg.name, msg.position, msg.velocity):
49            if name == pendulum_joint_name:
50                pendulum_position = position
51                pendulum_velocity = velocity
52            elif name == slider_joint_name:
53                slider_position = position
54                slider_velocity = velocity
55            
56        if pendulum_position is not None and slider_position is not None and pendulum_velocity is not None and slider_velocity is not None:
57            if not self.angle_threshold_exceeded and (pendulum_position > 1.48353 or pendulum_position < -1.48353):  # 約85度
58                self.angle_threshold_exceeded = True
59                exceed_time = self.get_clock().now()
60                time_difference = exceed_time - self.init_time
61                #print("TEST!!!!!!!!")
62                self.get_logger().info(f'Time difference from init to exceeding 85 degrees: {time_difference.nanoseconds / 1e9} seconds')
63                return  # 85度を超えた後は、努力値の計算と出力をスキップ
64
65            #self.get_logger().info(f'state: \n {self.state}')
66            self.calculate_and_publish_effort(pendulum_position, slider_position, pendulum_velocity, slider_velocity)
67
68    def calculate_and_publish_effort(self, pendulum_position, slider_position, pendulum_velocity, slider_velocity):
69        #self.state = np.array([[slider_position], [slider_velocity], [pendulum_position], [pendulum_velocity]])
70        #u = -(self.K @ (self.state - np.array([[0], [0], [math.pi], [0]]))).item(0)
71        #effort_value = u
72
73        self.state = np.array([[slider_position], [slider_velocity], [pendulum_position], [pendulum_velocity]])  # [x, x_dot, theta, theta_dot]
74        u = self.K @ self.state
75        effort_value = u.item(0)
76        
77        # 努力（出力）をパブリッシュ
78        msg = Float64MultiArray()
79        msg.data = [effort_value]
80        self.effort_publisher.publish(msg)
81        
82def main(args=None):
83    rclpy.init(args=args)
84    node = EffortControllerWithFeedback()
85
86    try:
87        rclpy.spin(node)
88    except KeyboardInterrupt:
89        pass
90    finally:
91        node.destroy_node()
92        rclpy.shutdown()
93
94if __name__ == '__main__':
95    main()
96

URDF
1<?xml version="1.0" encoding="utf-8"?>
2<robot name="Inverted_pendulum_ASM_v5">
3    
4    <link name="world"/>
5
6    <link name="slide_rail_v4:1">
7		<inertial>
8			<mass value="3.1243"/>
9            <origin rpy="0 0 0" xyz="0.0 0.0 0.007173366834170854"/>
10			<inertia ixx="0.000323965818520938" ixy="0.0" ixz="0.0" iyy="0.2604182814625208" iyz="0.0" izz="0.26062235102266657"/>
11		</inertial>
12		<collision name="slide_rail_v4:1_collision">
13			<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
14			<geometry>
15				<mesh filename="package://inverced_pendulum_v1/meshes/slide_rail_v4.stl" scale="0.001 0.001 0.001"/>
16			</geometry>
17		</collision>
18		<visual name="slide_rail_v4:1_visual">
19			<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
20			<geometry>
21				<mesh filename="package://inverced_pendulum_v1/meshes/slide_rail_v4.stl" scale="0.001 0.001 0.001"/>
22			</geometry>
23		</visual>
24	</link>
25    <gazebo reference="slide_rail_v4:1">
26        <material>Gazebo/Green</material>
27    </gazebo>
28
29    <!-- ワールドに対して静的ベースを固定するジョイント-->
30    <joint name="fixed_to_world" type="fixed">
31        <parent link="world"/>
32        <child link="slide_rail_v4:1"/>
33        <origin xyz="0 0 0" rpy="0 0 0"/>
34    </joint> 
35
36	<link name="slider_v4:1">
37		<inertial>
38			<mass value="0.11661244128735532"/>
39			<origin rpy="0 0 0" xyz="0 0.0025 0.01280549535934488"/>
40			<inertia ixx="2.5194396575474533e-05" ixy="0" ixz="0" iyy="1.9513766203100767e-05" iyz="0" izz="2.0436611160620293e-05"/>
41		</inertial>
42		<collision name="slider_v4:1_collision">
43			<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0025 0.0"/>
44			<geometry>
45				<mesh filename="package://inverced_pendulum_v1/meshes/slider_v4.stl" scale="0.001 0.001 0.001"/>
46			</geometry>
47		</collision>
48		<visual name="slider_v4:1_visual">
49			<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0025 0.0"/>
50			<geometry>
51				<mesh filename="package://inverced_pendulum_v1/meshes/slider_v4.stl" scale="0.001 0.001 0.001"/>
52			</geometry>
53		</visual>
54	</link>
55    <gazebo reference="slider_v4:1">
56        <material>Gazebo/Yellow</material>
57    </gazebo>
58
59
60    <joint name="rail_to_slider_joint" type="prismatic">
61        <origin xyz="0 0 0.015" rpy="0 0 0"/>
62        <parent link="slide_rail_v4:1"/>
63        <child link="slider_v4:1"/>
64        <axis xyz="1 0 0"/>
65        <limit effort="5000" velocity="1000" lower="-0.5" upper="0.5"/>
66    </joint>
67
68
69	<link name="pendulum_v6:1">
70		<inertial>
71			<mass value="2.4048913121650237"/>
72			<origin rpy="0 0 0" xyz="0 0 0.19588623318570866"/>
73			<inertia ixx="0.02523412148033483" ixy="0" ixz="0" iyy="0.025238043204504266" iyz="0" izz="0.0004887733280528608"/>
74		</inertial>
75		<collision name="pendulum_v6:1_collision">
76			<origin rpy="0.0 0.0 0.0" xyz="0 0.009 0"/>
77			<geometry>
78				<mesh filename="package://inverced_pendulum_v1/meshes/pendulum_v6.stl" scale="0.001 0.001 0.001"/>
79			</geometry>
80		</collision>
81		<visual name="pendulum_v6:1_visual">
82			<origin rpy="0.0 0 0.0" xyz="0 0.009 0"/>
83			<geometry>
84				<mesh filename="package://inverced_pendulum_v1/meshes/pendulum_v6.stl" scale="0.001 0.001 0.001"/>
85			</geometry>
86		</visual>
87	</link>
88
89    <gazebo reference="pendulum_v6:1">
90        <material>Gazebo/Purple</material>
91    </gazebo>
92
93	<joint name="slider_to_pendulum_joint" type="continuous">
94		<parent link="slider_v4:1"/>
95		<child link="pendulum_v6:1"/>
96		<origin rpy="0 0.0 0" xyz="0.0 -0.0065 0.02"/>
97		<axis xyz="0.0 -1.0 0.0"/>
98        <!--dynamics friction="0.01"/-->
99	</joint>
100
101    <!-- JOINT PUBLISHER -->
102    <gazebo>
103        <plugin name="pendulum_joint_state" filename="libgazebo_ros_joint_state_publisher.so">
104            <ros>
105                <remapping>~/out:=joint_states</remapping>
106            </ros>
107            <update_rate>30</update_rate>
108
109            <joint_name>rail_to_slider_joint</joint_name>
110            <joint_name>slider_to_pendulum_joint</joint_name>
111            
112        </plugin>
113    </gazebo>
114  
115    <!-- slider_position Control-->
116    <ros2_control name="GazeboSystem" type="system">
117        <hardware>
118            <plugin>gazebo_ros2_control/GazeboSystem</plugin>
119        </hardware>
120
121        <joint name="rail_to_slider_joint">
122            <command_interface name="effort">
123            </command_interface>
124            <state_interface name="position"/>
125            <state_interface name="velocity"/>
126            <state_interface name="effort"/>
127        </joint>
128
129    </ros2_control>
130
131    <gazebo>
132        <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
133            <parameters>$(find inverced_pendulum_v1)/config/controller_effort.yaml</parameters>
134            <robot_param_node>/my_robot_state_publisher_node</robot_param_node>
135        </plugin>
136    </gazebo>
137
138</robot>
139