Done with both parts of the assignment as well as uploaded videos :)

double_inverted/build/double_inverted/build/lib/double_inverted/double_inverted_dyanamics.py

@@ -0,0 +1,82 @@
+import rclpy
+from rclpy.node import Node
+import numpy as np
+import matplotlib.pyplot as plt
+import matplotlib.animation as animation
+
+class DoubleInvertedPendulumSimulator(Node):
+    def __init__(self):
+        super().__init__("double_inverted_pendulum_simulator")
+
+        # Initialize pendulum parameters
+        self.length1 = 1.0  # Length of the first pendulum
+        self.length2 = 1.0  # Length of the second pendulum
+        self.mass1 = 1.0    # Mass of the first pendulum
+        self.mass2 = 1.0    # Mass of the second pendulum
+        self.gravity = 9.81 # Acceleration due to gravity
+
+        # Initial state [theta1, omega1, theta2, omega2]
+        self.state = np.array([np.pi / 2, 0, np.pi / 2, 0])
+
+        # Time step for the simulation
+        self.dt = 0.01
+
+        # Setup the figure and axis for visualization
+        self.fig, self.ax = plt.subplots()
+        self.line, = self.ax.plot([], [], 'o-', lw=2)
+        self.ax.set_xlim(-2, 2)
+        self.ax.set_ylim(-2, 2)
+        self.ax.grid()
+
+        self.ani = animation.FuncAnimation(self.fig, self.update_plot, interval=50, blit=True)
+        plt.show()
+
+    def equations_of_motion(self, state):
+        theta1, omega1, theta2, omega2 = state
+
+        delta = theta2 - theta1
+
+        denom1 = (self.mass1 + self.mass2) * self.length1 - self.mass2 * self.length1 * np.cos(delta) * np.cos(delta)
+        denom2 = (self.length2 / self.length1) * denom1
+
+        a1 = (self.mass2 * self.length1 * omega1 * omega1 * np.sin(delta) * np.cos(delta) +
+              self.mass2 * self.gravity * np.sin(theta2) * np.cos(delta) +
+              self.mass2 * self.length2 * omega2 * omega2 * np.sin(delta) -
+              (self.mass1 + self.mass2) * self.gravity * np.sin(theta1)) / denom1
+
+        a2 = (- self.mass2 * self.length2 * omega2 * omega2 * np.sin(delta) * np.cos(delta) +
+              (self.mass1 + self.mass2) * self.gravity * np.sin(theta1) * np.cos(delta) -
+              (self.mass1 + self.mass2) * self.length1 * omega1 * omega1 * np.sin(delta) -
+              (self.mass1 + self.mass2) * self.gravity * np.sin(theta2)) / denom2
+
+        return np.array([omega1, a1, omega2, a2])
+
+    def step(self):
+        self.state += self.equations_of_motion(self.state) * self.dt
+
+    def update_plot(self, frame):
+        self.step()
+
+        theta1, _, theta2, _ = self.state
+
+        x1 = self.length1 * np.sin(theta1)
+        y1 = -self.length1 * np.cos(theta1)
+        x2 = x1 + self.length2 * np.sin(theta2)
+        y2 = y1 - self.length2 * np.cos(theta2)
+
+        self.line.set_data([0, x1, x2], [0, y1, y2])
+        return self.line,
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = DoubleInvertedPendulumSimulator()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()

double_inverted/build/double_inverted/colcon_build.rc

@@ -0,0 +1 @@
+0

double_inverted/build/double_inverted/colcon_command_prefix_setup_py.sh

@@ -0,0 +1 @@
+# generated from colcon_core/shell/template/command_prefix.sh.em

double_inverted/build/double_inverted/colcon_command_prefix_setup_py.sh.env

@@ -0,0 +1,57 @@
+AMENT_PREFIX_PATH=/opt/ros/galactic
+COLCON=1
+COLORTERM=truecolor
+DBUS_SESSION_BUS_ADDRESS=unix:path=/run/user/1000/bus,guid=76d3d2a54a87fb7fcc1215e866556bb3
+DBUS_STARTER_ADDRESS=unix:path=/run/user/1000/bus,guid=76d3d2a54a87fb7fcc1215e866556bb3
+DBUS_STARTER_BUS_TYPE=session
+DESKTOP_SESSION=ubuntu
+DISPLAY=:0
+GDMSESSION=ubuntu
+GNOME_DESKTOP_SESSION_ID=this-is-deprecated
+GNOME_SHELL_SESSION_MODE=ubuntu
+GNOME_TERMINAL_SCREEN=/org/gnome/Terminal/screen/25509364_879c_490d_a838_26e8c09adf24
+GNOME_TERMINAL_SERVICE=:1.643
+GPG_AGENT_INFO=/run/user/1000/gnupg/S.gpg-agent:0:1
+GTK_MODULES=gail:atk-bridge
+HOME=/home/aryan
+IM_CONFIG_PHASE=1
+INVOCATION_ID=685ce25ff4a54ba8a5f8e9295eb0a1ac
+JOURNAL_STREAM=8:45890
+LANG=en_IN
+LANGUAGE=en_IN:en
+LD_LIBRARY_PATH=/opt/ros/galactic/opt/yaml_cpp_vendor/lib:/opt/ros/galactic/opt/rviz_ogre_vendor/lib:/opt/ros/galactic/lib/x86_64-linux-gnu:/opt/ros/galactic/lib
+LESSCLOSE=/usr/bin/lesspipe %s %s
+LESSOPEN=| /usr/bin/lesspipe %s
+LOGNAME=aryan
+LS_COLORS=rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd=40;33;01:or=40;31;01:mi=00:su=37;41:sg=30;43:ca=30;41:tw=30;42:ow=34;42:st=37;44:ex=01;32:*.tar=01;31:*.tgz=01;31:*.arc=01;31:*.arj=01;31:*.taz=01;31:*.lha=01;31:*.lz4=01;31:*.lzh=01;31:*.lzma=01;31:*.tlz=01;31:*.txz=01;31:*.tzo=01;31:*.t7z=01;31:*.zip=01;31:*.z=01;31:*.dz=01;31:*.gz=01;31:*.lrz=01;31:*.lz=01;31:*.lzo=01;31:*.xz=01;31:*.zst=01;31:*.tzst=01;31:*.bz2=01;31:*.bz=01;31:*.tbz=01;31:*.tbz2=01;31:*.tz=01;31:*.deb=01;31:*.rpm=01;31:*.jar=01;31:*.war=01;31:*.ear=01;31:*.sar=01;31:*.rar=01;31:*.alz=01;31:*.ace=01;31:*.zoo=01;31:*.cpio=01;31:*.7z=01;31:*.rz=01;31:*.cab=01;31:*.wim=01;31:*.swm=01;31:*.dwm=01;31:*.esd=01;31:*.jpg=01;35:*.jpeg=01;35:*.mjpg=01;35:*.mjpeg=01;35:*.gif=01;35:*.bmp=01;35:*.pbm=01;35:*.pgm=01;35:*.ppm=01;35:*.tga=01;35:*.xbm=01;35:*.xpm=01;35:*.tif=01;35:*.tiff=01;35:*.png=01;35:*.svg=01;35:*.svgz=01;35:*.mng=01;35:*.pcx=01;35:*.mov=01;35:*.mpg=01;35:*.mpeg=01;35:*.m2v=01;35:*.mkv=01;35:*.webm=01;35:*.ogm=01;35:*.mp4=01;35:*.m4v=01;35:*.mp4v=01;35:*.vob=01;35:*.qt=01;35:*.nuv=01;35:*.wmv=01;35:*.asf=01;35:*.rm=01;35:*.rmvb=01;35:*.flc=01;35:*.avi=01;35:*.fli=01;35:*.flv=01;35:*.gl=01;35:*.dl=01;35:*.xcf=01;35:*.xwd=01;35:*.yuv=01;35:*.cgm=01;35:*.emf=01;35:*.ogv=01;35:*.ogx=01;35:*.aac=00;36:*.au=00;36:*.flac=00;36:*.m4a=00;36:*.mid=00;36:*.midi=00;36:*.mka=00;36:*.mp3=00;36:*.mpc=00;36:*.ogg=00;36:*.ra=00;36:*.wav=00;36:*.oga=00;36:*.opus=00;36:*.spx=00;36:*.xspf=00;36:
+MANAGERPID=1509
+PATH=/opt/ros/galactic/bin:/home/aryan/.local/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin
+PWD=/home/aryan/Inverted-pendulum-problems/double_inverted/build/double_inverted
+PYTHONPATH=/opt/ros/galactic/lib/python3.8/site-packages
+QT_ACCESSIBILITY=1
+QT_IM_MODULE=ibus
+ROS_DISTRO=galactic
+ROS_LOCALHOST_ONLY=0
+ROS_PYTHON_VERSION=3
+ROS_VERSION=2
+SESSION_MANAGER=local/aryan-Vivobook-ASUSLaptop-X1502ZA-X1502ZA:@/tmp/.ICE-unix/1647,unix/aryan-Vivobook-ASUSLaptop-X1502ZA-X1502ZA:/tmp/.ICE-unix/1647
+SHELL=/bin/bash
+SHLVL=1
+SSH_AGENT_PID=1607
+SSH_AUTH_SOCK=/run/user/1000/keyring/ssh
+TERM=xterm-256color
+USER=aryan
+USERNAME=aryan
+VTE_VERSION=6003
+WINDOWPATH=2
+XAUTHORITY=/run/user/1000/gdm/Xauthority
+XDG_CONFIG_DIRS=/etc/xdg/xdg-ubuntu:/etc/xdg
+XDG_CURRENT_DESKTOP=ubuntu:GNOME
+XDG_DATA_DIRS=/usr/share/ubuntu:/usr/local/share/:/usr/share/:/var/lib/snapd/desktop
+XDG_MENU_PREFIX=gnome-
+XDG_RUNTIME_DIR=/run/user/1000
+XDG_SESSION_CLASS=user
+XDG_SESSION_DESKTOP=ubuntu
+XDG_SESSION_TYPE=x11
+XMODIFIERS=@im=ibus
+_=/usr/bin/colcon

double_inverted/build/double_inverted/double_inverted.egg-info/PKG-INFO

@@ -0,0 +1,10 @@
+Metadata-Version: 1.2
+Name: double-inverted
+Version: 0.0.0
+Summary: TODO: Package description
+Home-page: UNKNOWN
+Maintainer: aryan
+Maintainer-email: aryankay1234@gmail.com
+License: TODO: License declaration
+Description: UNKNOWN
+Platform: UNKNOWN

double_inverted/build/double_inverted/double_inverted.egg-info/SOURCES.txt

@@ -0,0 +1,16 @@
+package.xml
+setup.cfg
+setup.py
+build/double_inverted/double_inverted.egg-info/PKG-INFO
+build/double_inverted/double_inverted.egg-info/SOURCES.txt
+build/double_inverted/double_inverted.egg-info/dependency_links.txt
+build/double_inverted/double_inverted.egg-info/entry_points.txt
+build/double_inverted/double_inverted.egg-info/requires.txt
+build/double_inverted/double_inverted.egg-info/top_level.txt
+build/double_inverted/double_inverted.egg-info/zip-safe
+double_inverted/__init__.py
+double_inverted/double_inverted_dyanamics.py
+resource/double_inverted
+test/test_copyright.py
+test/test_flake8.py
+test/test_pep257.py

double_inverted/build/double_inverted/double_inverted.egg-info/dependency_links.txt

@@ -0,0 +1 @@
+

double_inverted/build/double_inverted/double_inverted.egg-info/entry_points.txt

@@ -0,0 +1,3 @@
+[console_scripts]
+dyanamics = double_inverted.double_inverted_dyanamics:main
+

double_inverted/build/double_inverted/double_inverted.egg-info/requires.txt

@@ -0,0 +1 @@
+setuptools

double_inverted/build/double_inverted/double_inverted.egg-info/top_level.txt

@@ -0,0 +1 @@
+double_inverted

double_inverted/build/double_inverted/double_inverted.egg-info/zip-safe

@@ -0,0 +1 @@
+

double_inverted/build/double_inverted/install.log

@@ -0,0 +1,14 @@
+/home/aryan/Inverted-pendulum-problems/double_inverted/install/double_inverted/lib/python3.8/site-packages/double_inverted/double_inverted_dyanamics.py
+/home/aryan/Inverted-pendulum-problems/double_inverted/install/double_inverted/lib/python3.8/site-packages/double_inverted/__init__.py
+/home/aryan/Inverted-pendulum-problems/double_inverted/install/double_inverted/lib/python3.8/site-packages/double_inverted/__pycache__/double_inverted_dyanamics.cpython-38.pyc
+/home/aryan/Inverted-pendulum-problems/double_inverted/install/double_inverted/lib/python3.8/site-packages/double_inverted/__pycache__/__init__.cpython-38.pyc
+/home/aryan/Inverted-pendulum-problems/double_inverted/install/double_inverted/share/ament_index/resource_index/packages/double_inverted
+/home/aryan/Inverted-pendulum-problems/double_inverted/install/double_inverted/share/double_inverted/package.xml
+/home/aryan/Inverted-pendulum-problems/double_inverted/install/double_inverted/lib/python3.8/site-packages/double_inverted-0.0.0-py3.8.egg-info/PKG-INFO
+/home/aryan/Inverted-pendulum-problems/double_inverted/install/double_inverted/lib/python3.8/site-packages/double_inverted-0.0.0-py3.8.egg-info/entry_points.txt
+/home/aryan/Inverted-pendulum-problems/double_inverted/install/double_inverted/lib/python3.8/site-packages/double_inverted-0.0.0-py3.8.egg-info/dependency_links.txt
+/home/aryan/Inverted-pendulum-problems/double_inverted/install/double_inverted/lib/python3.8/site-packages/double_inverted-0.0.0-py3.8.egg-info/SOURCES.txt
+/home/aryan/Inverted-pendulum-problems/double_inverted/install/double_inverted/lib/python3.8/site-packages/double_inverted-0.0.0-py3.8.egg-info/requires.txt
+/home/aryan/Inverted-pendulum-problems/double_inverted/install/double_inverted/lib/python3.8/site-packages/double_inverted-0.0.0-py3.8.egg-info/zip-safe
+/home/aryan/Inverted-pendulum-problems/double_inverted/install/double_inverted/lib/python3.8/site-packages/double_inverted-0.0.0-py3.8.egg-info/top_level.txt
+/home/aryan/Inverted-pendulum-problems/double_inverted/install/double_inverted/lib/double_inverted/dyanamics

double_inverted/build/double_inverted/prefix_override/sitecustomize.py

@@ -0,0 +1,3 @@
+import sys
+sys.real_prefix = sys.prefix
+sys.prefix = sys.exec_prefix = '/home/aryan/Inverted-pendulum-problems/double_inverted/install/double_inverted'

double_inverted/double_inverted/double_inverted_dyanamics.py

@@ -0,0 +1,172 @@
+import numpy as np
+import matplotlib.pyplot as plt
+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 TorqueInput, States
+
+class Double_Inverted_Pendulum(Node):
+
+    # Initialise States : 
+    theta_1_init = np.pi - (np.random.rand() - 0.5) / 2
+    theta_2_init = np.pi / 3
+
+    theta1_dot_init = 0.0
+    theta2_dot_init = 0.0
+
+    # Param/s
+    mass1 = 1.0 # grams
+    mass2 = 0.1 # grams
+    l1 = 1.0 # metres
+    l2 = 1.0 # metres
+    a = 0.05 # metres
+    g = 9.81 # m / sec ^ 2
+
+    state_update_frequency = 500
+    state_update_timeperiod = 1 / state_update_frequency
+
+    feedback_frequency = 50
+
+    def __init__ (self) : 
+        super().__init__('main')
+
+        # Timers : 
+        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)
+
+        # Publishers : 
+        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)
+
+        # Attributes : 
+        self.t_start = time.time()
+        self.t_prev = time.time() - 0.0001
+        self.obj_id = 0
+
+        # States : 
+        self.theta1 = self.theta_1_init
+        self.theta2 = self.theta_2_init
+
+        self.theta1_dot = self.theta1_dot_init
+        self.theta2_dot = self.theta2_dot_init
+
+    def Update_Pendulum_States(self) :
+        dt = time.time() - self.t_prev
+        self.t_prev = time.time()
+
+        #Calculation of angular accelerations :)
+        Theta1_DoubleDot = (-self.g * (2 * self.mass1 + self.mass2) * np.sin(self.theta1)
+                     - self.mass2 * self.g * np.sin(self.theta1 - 2 * self.theta2)
+                     - 2 * np.sin(self.theta1 - self.theta2) * self.mass2 * (self.theta2_dot**2 * self.l2 + self.theta1_dot**2 * self.l1 * np.cos(self.theta1 - self.theta2))) / (self.l1 * (2 * self.mass1 + self.mass2 - self.mass2 * np.cos(2 * self.theta1 - 2 * self.theta2)))
+
+        Theta2_DoubleDot = (2 * np.sin(self.theta1 - self.theta2) * 
+                     (self.theta1_dot**2 * self.l1 * (self.mass1 + self.mass2) 
+                      + self.g * (self.mass1 + self.mass2) * np.cos(self.theta1) 
+                      + self.theta2_dot**2 * self.l2 * self.mass2 * np.cos(self.theta1 - self.theta2))) / (self.l2 * (2 * self.mass1 + self.mass2 - self.mass2 * np.cos(2 * self.theta1 - 2 * self.theta2)))
+
+        # Incrementing omegas
+        self.theta1_dot += Theta1_DoubleDot * dt
+        self.theta2_dot += Theta2_DoubleDot * dt
+
+        # Incrementing thetas
+        self.theta1 += self.theta1_dot * dt
+        self.theta2 += self.theta2_dot * dt
+
+        self.visualise_pendulum()
+
+        return
+
+    def Feedback(self):
+        states_msg1 = States()
+        states_msg1.theta = self.theta1
+        states_msg1.theta_dot = self.theta1_dot
+
+        states_msg2 = States()
+        states_msg2.theta = self.theta2
+        states_msg2.theta_dot = self.theta2_dot
+
+        self.feedback_pub1.publish(states_msg1)
+        self.feedback_pub2.publish(states_msg2)
+
+        return
+
+
+    def visualise_pendulum(self):
+        pendulum_marker1 = Marker()
+        pendulum_marker1.header.frame_id = "map"
+        pendulum_marker1.id = self.obj_id
+        pendulum_marker1.type = Marker.LINE_STRIP
+        pendulum_marker1.action = Marker.ADD
+        pendulum_marker1.pose.orientation.w = 1.0
+        pendulum_marker1.scale.x = 0.05  # Line width
+
+        # Set points
+        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_marker1.points = [point_1,
+                            point_2
+                        ]
+
+        pendulum_marker2 = Marker()
+        pendulum_marker2.header.frame_id = "map"
+        pendulum_marker2.id = self.obj_id
+        pendulum_marker2.type = Marker.LINE_STRIP
+        pendulum_marker2.action = Marker.ADD
+        pendulum_marker2.pose.orientation.w = 1.0
+        pendulum_marker2.scale.x = 0.05  # Line width
+
+        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_marker1.color.b = 1.0
+        pendulum_marker1.color.g = 1.0
+        pendulum_marker1.color.a = 1.0  # Alpha value
+        Duration_of_pendulum_marker = Duration()
+        Duration_of_pendulum_marker.sec = 0
+        Duration_of_pendulum_marker.nanosec = int(self.state_update_timeperiod * 1e+9)
+        pendulum_marker1.lifetime = Duration_of_pendulum_marker  # Permanent pendulum_marker
+        self.visualizer1.publish(pendulum_marker1)
+
+        pendulum_marker2.color.g = 1.0
+        pendulum_marker1.color.r = 1.0
+        pendulum_marker2.color.a = 1.0  # Alpha value
+        Duration_of_pendulum_marker = Duration()
+        Duration_of_pendulum_marker.sec = 0
+        Duration_of_pendulum_marker.nanosec = int(self.state_update_timeperiod * 1e+9)
+        pendulum_marker2.lifetime = Duration_of_pendulum_marker  # Permanent pendulum_marker
+        self.visualizer2.publish(pendulum_marker2)
+
+        self.obj_id += 1
+
+def main(args = None):
+
+    rclpy.init(args = args)
+    pendulum = Double_Inverted_Pendulum()
+    rclpy.spin(pendulum)
+
+    pendulum.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()