diff --git a/README.md b/README.md index edeae85..4aac6dc 100644 --- a/README.md +++ b/README.md @@ -8,10 +8,14 @@ #### 1) Single Inverted Pendulum - **Interface** - Crete a new node which subscribes to state feedback and publishes to torque input. Give torque and check if the pendulum is behaving appropriately (You can add this node to the launch file later) - **Balance** [Initial state - near upright] Write a controller to balance the inverted pendulum with initial state near upright position and not exactly upright +- **Balance initial condition** - Needs to changed manually in single_inverted/single_inverted/dynamics_sim.py + ![Output sample](illustrations/balance_initial_conditions.jpg) - **Swing-up** [Initial state - downward point at stable equilibrium] Write a controller to first swing up the pendulum, then balance on top. +- **Swing-up initial condition** - Needs to changed manually in single_inverted/single_inverted/dynamics_sim.py + ![Output sample](illustrations/swing_initial_conditions.jpg) #### 2) Double Inverted Pendulum -- Create a new pacakge with dynamics of an double inverted pendulum +- Create a new package with dynamics of an double inverted pendulum ## How-to 1. Clone this repository in /src folder of your ros2 workspace diff --git a/double_inverted_pendulum/.vscode/c_cpp_properties.json b/double_inverted_pendulum/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..1aba694 --- /dev/null +++ b/double_inverted_pendulum/.vscode/c_cpp_properties.json @@ -0,0 +1,24 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${default}", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/home/parthgoel/ros2_ws/install/zed_interfaces/include/**", + "/home/parthgoel/ros2_ws/install/eufs_msgs/include/**", + "/home/parthgoel/ros2_ws/install/custom_msgs/include/**", + "/opt/ros/galactic/include/**", + "/home/parthgoel/ros2_ws/src/parth_ros2/include/**", + "/usr/include/**" + ], + "name": "ROS", + "intelliSenseMode": "gcc-arm64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++14" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/double_inverted_pendulum/.vscode/settings.json b/double_inverted_pendulum/.vscode/settings.json new file mode 100644 index 0000000..f64438b --- /dev/null +++ b/double_inverted_pendulum/.vscode/settings.json @@ -0,0 +1,26 @@ +{ + "python.autoComplete.extraPaths": [ + "/home/parthgoel/ros2_ws/install/zed_interfaces/lib/python3.8/site-packages", + "/home/parthgoel/ros2_ws/build/single_inverted", + "/home/parthgoel/ros2_ws/install/single_inverted/lib/python3.8/site-packages", + "/home/parthgoel/ros2_ws/build/parth", + "/home/parthgoel/ros2_ws/install/parth/lib/python3.8/site-packages", + "/home/parthgoel/ros2_ws/install/eufs_msgs/lib/python3.8/site-packages", + "/home/parthgoel/ros2_ws/install/custom_msgs/lib/python3.8/site-packages", + "/home/parthgoel/ros2_ws/build/bag_recorder_nodes_py", + "/home/parthgoel/ros2_ws/install/bag_recorder_nodes_py/lib/python3.8/site-packages", + "/opt/ros/galactic/lib/python3.8/site-packages" + ], + "python.analysis.extraPaths": [ + "/home/parthgoel/ros2_ws/install/zed_interfaces/lib/python3.8/site-packages", + "/home/parthgoel/ros2_ws/build/single_inverted", + "/home/parthgoel/ros2_ws/install/single_inverted/lib/python3.8/site-packages", + "/home/parthgoel/ros2_ws/build/parth", + "/home/parthgoel/ros2_ws/install/parth/lib/python3.8/site-packages", + "/home/parthgoel/ros2_ws/install/eufs_msgs/lib/python3.8/site-packages", + "/home/parthgoel/ros2_ws/install/custom_msgs/lib/python3.8/site-packages", + "/home/parthgoel/ros2_ws/build/bag_recorder_nodes_py", + "/home/parthgoel/ros2_ws/install/bag_recorder_nodes_py/lib/python3.8/site-packages", + "/opt/ros/galactic/lib/python3.8/site-packages" + ] +} \ No newline at end of file diff --git a/double_inverted_pendulum/double_inverted_pendulum/__init__.py b/double_inverted_pendulum/double_inverted_pendulum/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/double_inverted_pendulum/double_inverted_pendulum/double_pendulum.py b/double_inverted_pendulum/double_inverted_pendulum/double_pendulum.py new file mode 100644 index 0000000..75133cf --- /dev/null +++ b/double_inverted_pendulum/double_inverted_pendulum/double_pendulum.py @@ -0,0 +1,152 @@ +import numpy as np +import time +from math import sin, cos + +import rclpy +from rclpy.node import Node + +from builtin_interfaces.msg import Duration +from visualization_msgs.msg import Marker +from geometry_msgs.msg import Point + +from custom_msgs.msg import States + +class double_pendulum(Node): + theta0_1 = np.pi + 0.01 + theta_dot0_1 = 0.0 + theta0_2 = np.pi/4 # pi, -pi represents upright + theta_dot0_2 = 0.0 + + state_update_frequency = 3000 + state_update_timeperiod = 1 / state_update_frequency + + feedback_frequency = 50 + + def __init__(self): + super().__init__('main') + self.m1 = 1.0 + self.m2= 1.0 + self.g = 9.81 + self.l1= 1.0 + self.l2=1.0 + + update_states_timer = self.create_timer(1 / self.state_update_frequency, self.update_pendulum_states) + feedback_timer = self.create_timer(1 / self.feedback_frequency, self.feedback) + self.visualizer1 = self.create_publisher(Marker, '/pendulum_viz1', 1) + self.visualizer2 = self.create_publisher(Marker, '/pendulum_viz2', 1) + self.feedback_pub1 = self.create_publisher(States, '/state_feedback1', 1) + self.feedback_pub2 = self.create_publisher(States, '/state_feedback2', 1) + + self.t_start = time.time() + self.t_prev = time.time() - 0.0001 + self.obj_id = 0 + + self.theta1 = self.theta0_1 + self.omega1 = self.theta_dot0_1 + self.theta2 = self.theta0_2 + self.omega2 = self.theta_dot0_2 + self.get_logger().info('Double Inverted Pendulum node initialized') + self.get_logger().info('Accepting Input') + self.get_logger().info('Publishing Feedback') + + def update_pendulum_states(self): + + dt = time.time() - self.t_prev + self.t_prev = time.time() + + domega1_dt = (-self.g * (2 * self.m1 + self.m2) * np.sin(self.theta1) - self.m2 * self.g * np.sin(self.theta1 - 2 * self.theta2) - 2 * np.sin(self.theta1 - self.theta2) * self.m2 * (self.omega2 ** 2 * self.l2 + self.omega1 ** 2 * self.l1 * np.cos(self.theta1 - self.theta2))) / (self.l1 * (2 * self.m1 + self.m2 - self.m2 * np.cos(2 * self.theta1 - 2 * self.theta2))) + domega2_dt = (2 * np.sin(self.theta1 - self.theta2) * (self.omega1 ** 2 * self.l1 * (self.m1 + self.m2) + self.g * (self.m1 + self.m2) * np.cos(self.theta1) + self.omega2 ** 2 * self.l2 * self.m2 * np.cos(self.theta1 - self.theta2))) / (self.l2 * (2 * self.m1 + self.m2 - self.m2 * np.cos(2 * self.theta1 - 2 * self.theta2))) + + + self.omega1 += domega1_dt * dt + self.omega2 += domega2_dt * dt + + + self.theta1 += self.omega1 * dt + self.theta2 += self.omega2 * dt + + self.visualize_pendulum() + + return + + def feedback(self): + states_msg1 = States() + states_msg1.theta = self.theta1 + states_msg1.theta_dot = self.omega1 + states_msg2 = States() + states_msg2.theta = self.theta2 + states_msg2.theta_dot = self.omega2 + self.feedback_pub1.publish(states_msg1) + self.feedback_pub2.publish(states_msg2) + return + + def visualize_pendulum(self): + pendulum_marker = Marker() + pendulum_marker.header.frame_id = "map" + pendulum_marker.id = 0 + pendulum_marker.type = Marker.LINE_STRIP + pendulum_marker.action = Marker.ADD + pendulum_marker.pose.orientation.w = 1.0 + pendulum_marker.scale.x = 0.05 + + + point_1 = Point() + point_1.x = 0.0 + point_1.y = 0.0 + point_1.z = 0.0 + + point_2 = Point() + point_2.x = self.l1 * sin(self.theta1) + point_2.y = - self.l1 * cos(self.theta1) + point_2.z = 0.0 + pendulum_marker.points = [point_1, + point_2 + ] + + pendulum_marker.color.r = 1.0 + pendulum_marker.color.a = 1.0 + Duration_of_pendulum_marker = Duration() + Duration_of_pendulum_marker.sec = 0 + Duration_of_pendulum_marker.nanosec = int(self.state_update_timeperiod * 1e+9) + pendulum_marker.lifetime = Duration_of_pendulum_marker + self.visualizer1.publish(pendulum_marker) + + pendulum_marker2 = Marker() + pendulum_marker2.header.frame_id = "map" + pendulum_marker2.id = 1 + pendulum_marker2.type = Marker.LINE_STRIP + pendulum_marker2.action = Marker.ADD + pendulum_marker2.pose.orientation.w = 1.0 + pendulum_marker2.scale.x = 0.05 + + point_3 = Point() + point_3.x = point_2.x+(self.l2 * sin(self.theta2)) + point_3.y = point_2.y-(self.l2 * cos(self.theta2)) + point_3.z = 0.0 + pendulum_marker2.points = [point_2, + point_3 + ] + + pendulum_marker2.color.r = 1.0 + pendulum_marker2.color.a = 1.0 + Duration_of_pendulum_marker2 = Duration() + Duration_of_pendulum_marker2.sec = 0 + Duration_of_pendulum_marker2.nanosec = int(self.state_update_timeperiod * 1e+9) + pendulum_marker2.lifetime = Duration_of_pendulum_marker2 + self.visualizer2.publish(pendulum_marker2) + + self.obj_id += 1 + + + +def main(args = None): + + rclpy.init(args = args) + pendulum_ = double_pendulum() + rclpy.spin(pendulum_) + pendulum_.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/double_inverted_pendulum/package.xml b/double_inverted_pendulum/package.xml new file mode 100644 index 0000000..1dbfac6 --- /dev/null +++ b/double_inverted_pendulum/package.xml @@ -0,0 +1,32 @@ + + + + double_inverted_pendulum + 0.0.0 + TODO: Package description + parthgoel + TODO: License declaration + + rclpy + geometry_msgs + turtlesim + numpy + math + sensor_msgs + eufs_msgs + visualization_msgs + builtin_interfaces_msgs + message_filters + custom_msgs + time + + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/double_inverted_pendulum/resource/double_inverted_pendulum b/double_inverted_pendulum/resource/double_inverted_pendulum new file mode 100644 index 0000000..e69de29 diff --git a/double_inverted_pendulum/setup.cfg b/double_inverted_pendulum/setup.cfg new file mode 100644 index 0000000..029cecd --- /dev/null +++ b/double_inverted_pendulum/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/double_inverted_pendulum +[install] +install_scripts=$base/lib/double_inverted_pendulum diff --git a/double_inverted_pendulum/setup.py b/double_inverted_pendulum/setup.py new file mode 100644 index 0000000..66a323f --- /dev/null +++ b/double_inverted_pendulum/setup.py @@ -0,0 +1,26 @@ +from setuptools import setup + +package_name = 'double_inverted_pendulum' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='parthgoel', + maintainer_email='parthgoel9@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + "double_pendulum=double_inverted_pendulum.double_pendulum:main" + ], + }, +) diff --git a/double_inverted_pendulum/test/test_copyright.py b/double_inverted_pendulum/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/double_inverted_pendulum/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/double_inverted_pendulum/test/test_flake8.py b/double_inverted_pendulum/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/double_inverted_pendulum/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/double_inverted_pendulum/test/test_pep257.py b/double_inverted_pendulum/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/double_inverted_pendulum/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/illustrations/balance_initial_conditions.jpg b/illustrations/balance_initial_conditions.jpg new file mode 100644 index 0000000..6a61169 Binary files /dev/null and b/illustrations/balance_initial_conditions.jpg differ diff --git a/illustrations/swing_initial_conditions.jpg b/illustrations/swing_initial_conditions.jpg new file mode 100644 index 0000000..f1c20b3 Binary files /dev/null and b/illustrations/swing_initial_conditions.jpg differ diff --git a/single_inverted/launch/single_inverted_pendulum.launch.py b/single_inverted/launch/single_inverted_pendulum.launch.py index 0e7c01b..46b8195 100644 --- a/single_inverted/launch/single_inverted_pendulum.launch.py +++ b/single_inverted/launch/single_inverted_pendulum.launch.py @@ -22,7 +22,10 @@ def generate_launch_description(): executable = 'dynamics_sim' ) + + + return LaunchDescription([ visualizer, dynamics - ]) \ No newline at end of file + ]) diff --git a/single_inverted/setup.py b/single_inverted/setup.py index d02acd7..e1bfc66 100644 --- a/single_inverted/setup.py +++ b/single_inverted/setup.py @@ -26,7 +26,10 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'dynamics_sim = single_inverted.dynamics_sim:main' + 'dynamics_sim = single_inverted.dynamics_sim:main', + "pendulum_interface=single_inverted.pendulum_interface:main", + "pendulum_balance=single_inverted.pendulum_balance:main", + "pendulum_swing=single_inverted.pendulum_swing:main" ], }, ) diff --git a/single_inverted/single_inverted/dynamics_sim.py b/single_inverted/single_inverted/dynamics_sim.py index e62365b..2956904 100644 --- a/single_inverted/single_inverted/dynamics_sim.py +++ b/single_inverted/single_inverted/dynamics_sim.py @@ -153,7 +153,6 @@ def visualize_pendulum(self): def update_input_torque(self, msg): self.torque_value = max(-5,min(5,msg.torque_value)) - return diff --git a/single_inverted/single_inverted/pendulum_balance.py b/single_inverted/single_inverted/pendulum_balance.py new file mode 100644 index 0000000..0a6529d --- /dev/null +++ b/single_inverted/single_inverted/pendulum_balance.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python3 +import rclpy +import numpy as np +from rclpy.node import Node +from custom_msgs.msg import States,TorqueInput +import matplotlib.pyplot as plt +import time +class pendulum_balance(Node): + def __init__(self): + super().__init__("pose_subscriber") + self.subscriber = self.create_subscription(States,"/state_feedback",self.callback1,10) + self.publisher = self.create_publisher(TorqueInput,"/torque_input",10) + #self.timer = self.create_timer(4,self.send_torque) + self.theta = np.pi - (np.random.rand() - 0.5) / 2 + self.theta = (self.theta + np.pi)%(2*np.pi) - np.pi + self.theta_dot = 0 + self.torque = 0 + self.setpoint = np.pi + + self.t_start = time.time() + self.t_prev = time.time() - 0.0001 + self.integral = 0 + self.previous_error = self.setpoint - self.theta + + + self.Kp= 50 + self.Ki = 0 + self.Kd = 6 + + self.theta_values = [] + self.time_values = [] + + + def callback1(self,msg:States): + self.theta = msg.theta + self.theta_dot = msg.theta_dot + + t = TorqueInput() + if self.theta>=0: + error = (np.pi - self.theta) + if self.theta<0: + error = -(np.pi + self.theta) + + p = self.Kp * error + dt = time.time() - self.t_prev + self.t_prev = time.time() + self.integral += error* dt + i = self.Ki * self.integral + d = self.Kd * (error - self.previous_error)/dt + self.previous_error = error + t.torque_value = (p + i + d) + if t.torque_value>5: + t.torque_value = 5.0 + if t.torque_value<-5: + t.torque_value = -5.0 + self.publisher.publish(t) + self.get_logger().info("theta: "+ str(self.theta)) + #self.get_logger().info("theta_dot: "+ str(self.theta_dot)) + self.theta_values.append(self.theta) + self.time_values.append(time.time() - self.t_start) + + def plot_theta_vs_time(self): + plt.plot(self.time_values, self.theta_values) + plt.xlabel('Time (s)') + plt.ylabel('Theta (rad)') + plt.title('Theta vs Time') + plt.grid(True) + plt.show() + +def main(args=None): + rclpy.init(args=args) + node = pendulum_balance() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.plot_theta_vs_time() + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/single_inverted/single_inverted/pendulum_interface.py b/single_inverted/single_inverted/pendulum_interface.py new file mode 100644 index 0000000..718a8bb --- /dev/null +++ b/single_inverted/single_inverted/pendulum_interface.py @@ -0,0 +1,29 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from custom_msgs.msg import States,TorqueInput + +class pendulum_interface(Node): + def __init__(self): + super().__init__("pose_subscriber") + self.subscriber = self.create_subscription(States,"/state_feedback",self.callback1,10) + self.publisher = self.create_publisher(TorqueInput,"/torque_input",10) + self.timer = self.create_timer(1/3000,self.send_torque) + def callback1(self,msg:States): + self.get_logger().info("theta: "+ str(msg.theta)) + self.get_logger().info("ang_vel: " + str(msg.theta_dot)) + + def send_torque(self): + t = TorqueInput() + t.torque_value = 1.0 + self.publisher.publish(t) + + +def main(args=None): + rclpy.init(args=args) + node = pendulum_interface() + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/single_inverted/single_inverted/pendulum_swing.py b/single_inverted/single_inverted/pendulum_swing.py new file mode 100644 index 0000000..0ca3f9d --- /dev/null +++ b/single_inverted/single_inverted/pendulum_swing.py @@ -0,0 +1,77 @@ +#!/usr/bin/env python3 +import rclpy +import numpy as np +from rclpy.node import Node +from custom_msgs.msg import States,TorqueInput +import matplotlib.pyplot as plt +import time +class pendulum_swing(Node): + def __init__(self): + super().__init__("pose_subscriber") + self.subscriber = self.create_subscription(States,"/state_feedback",self.callback1,10) + self.publisher = self.create_publisher(TorqueInput,"/torque_input",10) + #self.timer = self.create_timer(4,self.send_torque) + self.theta = 0 - (np.random.rand() - 0.5) / 2 + self.theta = (self.theta + np.pi)%(2*np.pi) - np.pi + self.theta_dot = 0 + self.torque = 0 + self.setpoint = np.pi + + self.t_start = time.time() + self.t_prev = time.time() - 0.0001 + self.integral = 0 + self.previous_error = self.setpoint - self.theta + self.previous_theta = 0 + self.torque_prev = 5.0 + + + self.Kp= 50 + self.Ki = 0 + self.Kd = 6 + + self.theta_values = [] + self.time_values = [] + + self.swing_up_completed = False + + + def callback1(self,msg:States): + self.theta = msg.theta + self.theta_dot = msg.theta_dot + t = TorqueInput() + if not self.swing_up_completed: + if abs(self.theta) < 2*np.pi/3: + if abs(self.theta - self.previous_theta) > 0.1: + t.torque_value =0.0 + if abs(self.theta - self.previous_theta) < 0.001: + self.torque_prev *= -1 + t.torque_value = self.previous_theta + if abs(self.theta)>2*np.pi/3: + self.swing_up_completed = True + + + # PID controller for stabilization + error = (np.pi - self.theta) if self.theta >= 0 else -(np.pi + self.theta) + p = self.Kp * error + dt = time.time() - self.t_prev + self.t_prev = time.time() + self.integral += error * dt + i = self.Ki * self.integral + d = self.Kd * (error - self.previous_error) / dt + self.previous_error = error + t.torque_value = p + i + d + + # Clip the torque to the maximum allowed value + t.torque_value = max(min(t.torque_value, 5.0), -5.0) + self.publisher.publish(t) + + +def main(args=None): + rclpy.init(args=args) + node = pendulum_swing() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main()