From dc95794a8eca80bb2009fe3a6992d51e5c5ff9ff Mon Sep 17 00:00:00 2001
From: Atharav1805 <168408064+Atharav1805@users.noreply.github.com>
Date: Tue, 4 Jun 2024 02:26:47 +0530
Subject: [PATCH 01/22] Controller node for balancing

---
 controller_node.py | 119 +++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 119 insertions(+)
 create mode 100644 controller_node.py

diff --git a/controller_node.py b/controller_node.py
new file mode 100644
index 0000000..ea49dcc
--- /dev/null
+++ b/controller_node.py
@@ -0,0 +1,119 @@
+#!/usr/bin/env python3
+
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from custom_msgs.msg import States
+from custom_msgs.msg import TorqueInput
+import matplotlib.pyplot as plt
+import time
+
+
+class ControllerNode(Node):
+
+    def __init__(self):
+        super().__init__('controller_node')
+        self.controller_node_subs = self.create_subscription(States, "/state_feedback", self.pose_callback, 10) # create a subscriber with the name controller_node_subs that subscribes to the state_feedback topic
+        self.controller_node_pub = self.create_publisher(TorqueInput, "/torque_input", 10) # create a publisher with the name controller_node_pub that publishes to the torque_input topic
+        #self.timer = self.create_timer(0.02, self.send_torque_info) # create a timer that calls the send_torque_info function every 0.01 seconds
+        #Set the state variables
+        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.0
+        self.target_angle = np.pi
+
+        # Create a list to store the theta and time values
+        self.theta_values = []
+        self.time_values = []
+
+        self.start_time = time.time()
+        self.prev_time = self.start_time
+        self.previous_error = self.target_angle - self.theta
+        self.integral = 0
+
+        self.get_logger().info("Controller node has been started")
+
+        #Inititalize lists to store theta and time values
+        
+
+
+    def pose_callback(self, msg: States):
+        #self.get_logger().info("Received state: " + str(msg.theta) + ", " + str(msg.theta_dot))
+        self.theta = msg.theta
+        self.theta_dot = msg.theta_dot
+
+        if self.theta < 0:
+            error = -self.target_angle - self.theta
+        if self.theta >= 0:
+            error = self.target_angle - self.theta
+
+        self.PID(error)
+        self.theta_values.append(self.theta) # append the theta value to the list
+        self.time_values.append(time.time() - self.start_time) # append the time value to the list
+        
+
+    def send_torque_info(self): # create a function that sends the torque value to the simulation
+
+        self.torque = max(-5.0, min(5.0, self.torque)) # limit the torque value to be between -5 and 5
+
+        #Publish the torque value
+        msg = TorqueInput() # create a TorqueInput message
+        msg.torque_value = self.torque
+        self.controller_node_pub.publish(msg) # publish the torque value to the topic
+        self.get_logger().info("Sent torque: " + str(self.torque)) # log the torque value
+
+
+    def PID(self,error):
+
+        # Set the PID gains
+        Kp = 48
+        Ki = 0.01
+        Kd = 0.6
+
+        # Compute the time difference
+        dt = time.time() - self.prev_time
+        self.prev_time = time.time() # update the previous time
+
+        self.integral += error * dt
+        derivative = (error - self.previous_error) / dt
+        self.previous_error = error
+
+        p = Kp * error
+        i = Ki * self.integral
+        d = Kd * derivative
+
+        self.torque = p + i + d
+        self.send_torque_info()
+
+
+    def plot_graph(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 = ControllerNode()
+    
+    try: 
+
+        rclpy.spin(node)
+    
+    except KeyboardInterrupt:
+
+        node.get_logger().info("Keyboard Interrupt detected...shutting the node down")
+        node.plot_graph()
+        pass
+    
+    rclpy.shutdown()
+
+
+
+   
\ No newline at end of file

From 005487d3997c1df78cd691bda1fe8115417e3204 Mon Sep 17 00:00:00 2001
From: Atharav1805 <168408064+Atharav1805@users.noreply.github.com>
Date: Thu, 6 Jun 2024 01:53:51 +0530
Subject: [PATCH 02/22] updated dt calculation

---
 controller_node.py | 13 +++++--------
 1 file changed, 5 insertions(+), 8 deletions(-)

diff --git a/controller_node.py b/controller_node.py
index ea49dcc..9627bd5 100644
--- a/controller_node.py
+++ b/controller_node.py
@@ -27,17 +27,14 @@ def __init__(self):
         self.theta_values = []
         self.time_values = []
 
-        self.start_time = time.time()
-        self.prev_time = self.start_time
+        self.node_start_time = time.time()
+        self.prev_time = time.time()
         self.previous_error = self.target_angle - self.theta
         self.integral = 0
 
         self.get_logger().info("Controller node has been started")
 
-        #Inititalize lists to store theta and time values
         
-
-
     def pose_callback(self, msg: States):
         #self.get_logger().info("Received state: " + str(msg.theta) + ", " + str(msg.theta_dot))
         self.theta = msg.theta
@@ -50,7 +47,7 @@ def pose_callback(self, msg: States):
 
         self.PID(error)
         self.theta_values.append(self.theta) # append the theta value to the list
-        self.time_values.append(time.time() - self.start_time) # append the time value to the list
+        self.time_values.append(time.time() - self.node_start_time) # append the time value to the list
         
 
     def send_torque_info(self): # create a function that sends the torque value to the simulation
@@ -67,8 +64,8 @@ def send_torque_info(self): # create a function that sends the torque value to t
     def PID(self,error):
 
         # Set the PID gains
-        Kp = 48
-        Ki = 0.01
+        Kp = 50.0
+        Ki = 0.001
         Kd = 0.6
 
         # Compute the time difference

From c4adc9fe583c80646fc0130b8f0cddfa0b228ac6 Mon Sep 17 00:00:00 2001
From: Atharav1805 <168408064+Atharav1805@users.noreply.github.com>
Date: Thu, 6 Jun 2024 01:56:38 +0530
Subject: [PATCH 03/22] changes initialisation of integral error

---
 controller_node.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/controller_node.py b/controller_node.py
index 9627bd5..a53b7ab 100644
--- a/controller_node.py
+++ b/controller_node.py
@@ -30,7 +30,7 @@ def __init__(self):
         self.node_start_time = time.time()
         self.prev_time = time.time()
         self.previous_error = self.target_angle - self.theta
-        self.integral = 0
+        self.integral = 0.0
 
         self.get_logger().info("Controller node has been started")
 

From 2794c3596ed5128890a4c720a4cd731f30251a9a Mon Sep 17 00:00:00 2001
From: Atharav1805 <168408064+Atharav1805@users.noreply.github.com>
Date: Thu, 6 Jun 2024 02:41:55 +0530
Subject: [PATCH 04/22] Controller node for simulating swing up and balance

---
 controller_node_swing_up.py | 147 ++++++++++++++++++++++++++++++++++++
 1 file changed, 147 insertions(+)
 create mode 100644 controller_node_swing_up.py

diff --git a/controller_node_swing_up.py b/controller_node_swing_up.py
new file mode 100644
index 0000000..4fec5a4
--- /dev/null
+++ b/controller_node_swing_up.py
@@ -0,0 +1,147 @@
+#!/usr/bin/env python3
+
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from custom_msgs.msg import States
+from custom_msgs.msg import TorqueInput
+import matplotlib.pyplot as plt
+import time
+
+
+class ControllerNode(Node):
+
+    def __init__(self):
+        super().__init__('controller_node')
+        self.controller_node_subs = self.create_subscription(States, "/state_feedback", self.pose_callback, 10) # create a subscriber with the name controller_node_subs that subscribes to the state_feedback topic
+        self.controller_node_pub = self.create_publisher(TorqueInput, "/torque_input", 10) # create a publisher with the name controller_node_pub that publishes to the torque_input topic
+        #self.timer = self.create_timer(0.02, self.send_torque_info) # create a timer that calls the send_torque_info function every 0.01 seconds
+        #Set the state variables
+        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.0
+        self.target_angle = np.pi
+
+        # Create a list to store the theta and time values
+        self.theta_values = []
+        self.time_values = []
+
+        self.start_time = time.time()
+        self.prev_time = self.start_time
+        self.previous_error = self.target_angle - self.theta
+        self.integral = 0.0
+        
+        self.previous_theta = self.theta
+        self.previous_torque = 5.0
+
+        self.swing_up_complete = False
+
+
+        self.get_logger().info("Controller node has been started")
+
+    
+        
+
+
+    def pose_callback(self, msg: States):
+        #self.get_logger().info("Received state: " + str(msg.theta) + ", " + str(msg.theta_dot))
+        self.theta = msg.theta
+        self.theta_dot = msg.theta_dot
+
+        if self.swing_up_complete == False:
+            self.swing_up()
+        
+        if abs(self.theta) >= np.pi -0.25 :
+            self.swing_up_complete = True
+
+        if self.swing_up_complete == True:
+            self.PID_balance()
+        
+        self.theta_values.append(self.theta) # append the theta value to the list
+        self.time_values.append(time.time() - self.start_time) # append the time value to the list
+
+
+    def swing_up(self):
+
+        if abs(self.theta - self.previous_theta) >= 0.5:
+            self.torque = 0.0
+
+        elif abs(self.theta) < np.pi / 1.5:
+            if abs(self.theta - self.previous_theta) < 0.05:
+                self.previous_torque *= -1
+                self.torque = self.previous_torque
+        
+        self.send_torque_info()
+        
+
+    def send_torque_info(self): # create a function that sends the torque value to the simulation
+
+        self.torque = max(-5.0, min(5.0, self.torque)) # limit the torque value to be between -5 and 5
+
+        #Publish the torque value
+        msg = TorqueInput() # create a TorqueInput message
+        msg.torque_value = self.torque
+        self.controller_node_pub.publish(msg) # publish the torque value to the topic
+        self.get_logger().info("Sent torque: " + str(self.torque)) # log the torque value
+
+    
+    def PID_balance(self):
+
+        # Set the PID gains
+        Kp = 50.0
+        Ki = 0.01
+        Kd = 0.6
+
+        if self.theta < 0:
+            error = -self.target_angle - self.theta
+        if self.theta >= 0:
+            error = self.target_angle - self.theta
+
+        # Compute the time difference
+        dt = time.time() - self.prev_time
+        self.prev_time = time.time() # update the previous time
+
+        self.integral += error * dt
+        derivative = (error - self.previous_error) / dt
+        self.previous_error = error
+
+        p = Kp * error
+        i = Ki * self.integral
+        d = Kd * derivative
+
+        self.torque = p + i + d 
+        self.send_torque_info()
+
+
+    def plot_graph(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 = ControllerNode()
+    
+    try: 
+
+        rclpy.spin(node)
+    
+    except KeyboardInterrupt:
+
+        node.get_logger().info("Keyboard Interrupt detected...shutting the node down")
+        node.plot_graph()
+        node.destroy_node()
+    
+        
+    rclpy.shutdown()
+
+
+
+   
\ No newline at end of file

From 7f9e84d4b3b79f43e17ba2c79302c669c29a044f Mon Sep 17 00:00:00 2001
From: Atharav1805 <168408064+Atharav1805@users.noreply.github.com>
Date: Sat, 8 Jun 2024 05:56:37 +0530
Subject: [PATCH 05/22] Add files via upload

---
 controller_node.py          |  6 ++++--
 controller_node_swing_up.py | 13 +++++++------
 2 files changed, 11 insertions(+), 8 deletions(-)

diff --git a/controller_node.py b/controller_node.py
index a53b7ab..5bdd0ea 100644
--- a/controller_node.py
+++ b/controller_node.py
@@ -40,6 +40,8 @@ def pose_callback(self, msg: States):
         self.theta = msg.theta
         self.theta_dot = msg.theta_dot
 
+
+
         if self.theta < 0:
             error = -self.target_angle - self.theta
         if self.theta >= 0:
@@ -77,10 +79,10 @@ def PID(self,error):
         self.previous_error = error
 
         p = Kp * error
-        i = Ki * self.integral
+        #i = Ki * self.integral
         d = Kd * derivative
 
-        self.torque = p + i + d
+        self.torque = p + d # calculate the torque value
         self.send_torque_info()
 
 
diff --git a/controller_node_swing_up.py b/controller_node_swing_up.py
index 4fec5a4..16c0acd 100644
--- a/controller_node_swing_up.py
+++ b/controller_node_swing_up.py
@@ -52,8 +52,9 @@ def pose_callback(self, msg: States):
         if self.swing_up_complete == False:
             self.swing_up()
         
-        if abs(self.theta) >= np.pi -0.25 :
+        if abs(self.theta) >= np.pi -0.4 :
             self.swing_up_complete = True
+            self.get_logger().info("Swing up complete")
 
         if self.swing_up_complete == True:
             self.PID_balance()
@@ -64,7 +65,7 @@ def pose_callback(self, msg: States):
 
     def swing_up(self):
 
-        if abs(self.theta - self.previous_theta) >= 0.5:
+        if abs(self.theta - self.previous_theta) >= 0.4:
             self.torque = 0.0
 
         elif abs(self.theta) < np.pi / 1.5:
@@ -90,8 +91,8 @@ def PID_balance(self):
 
         # Set the PID gains
         Kp = 50.0
-        Ki = 0.01
-        Kd = 0.6
+        #Ki = 0.01
+        Kd = 6.0
 
         if self.theta < 0:
             error = -self.target_angle - self.theta
@@ -107,10 +108,10 @@ def PID_balance(self):
         self.previous_error = error
 
         p = Kp * error
-        i = Ki * self.integral
+        #i = Ki * self.integral
         d = Kd * derivative
 
-        self.torque = p + i + d 
+        self.torque = p + d 
         self.send_torque_info()
 
 

From 8351440923b628d62c2f28542aade67b3fdfe155 Mon Sep 17 00:00:00 2001
From: Atharav1805 <168408064+Atharav1805@users.noreply.github.com>
Date: Tue, 11 Jun 2024 17:47:41 +0530
Subject: [PATCH 06/22] Updated PID gains

---
 controller_node.py          | 6 +++---
 controller_node_swing_up.py | 2 +-
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/controller_node.py b/controller_node.py
index 5bdd0ea..e33ea44 100644
--- a/controller_node.py
+++ b/controller_node.py
@@ -68,7 +68,7 @@ def PID(self,error):
         # Set the PID gains
         Kp = 50.0
         Ki = 0.001
-        Kd = 0.6
+        Kd = 6.0
 
         # Compute the time difference
         dt = time.time() - self.prev_time
@@ -79,10 +79,10 @@ def PID(self,error):
         self.previous_error = error
 
         p = Kp * error
-        #i = Ki * self.integral
+        i = Ki * self.integral
         d = Kd * derivative
 
-        self.torque = p + d # calculate the torque value
+        self.torque = p + i + d # calculate the torque value
         self.send_torque_info()
 
 
diff --git a/controller_node_swing_up.py b/controller_node_swing_up.py
index 16c0acd..eaa739a 100644
--- a/controller_node_swing_up.py
+++ b/controller_node_swing_up.py
@@ -91,7 +91,7 @@ def PID_balance(self):
 
         # Set the PID gains
         Kp = 50.0
-        #Ki = 0.01
+        Ki = 0.01
         Kd = 6.0
 
         if self.theta < 0:

From 1eea8bdd9eeb84b4998dd992a302911391602e5b Mon Sep 17 00:00:00 2001
From: Atharav1805 <168408064+Atharav1805@users.noreply.github.com>
Date: Tue, 11 Jun 2024 18:29:23 +0530
Subject: [PATCH 07/22] Create Results.md

---
 Results.md | 7 +++++++
 1 file changed, 7 insertions(+)
 create mode 100644 Results.md

diff --git a/Results.md b/Results.md
new file mode 100644
index 0000000..bae0254
--- /dev/null
+++ b/Results.md
@@ -0,0 +1,7 @@
+1. Balance:
+[balance.webm](https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/9e45ce17-7df3-47fb-a1ae-0e2064d48eb7)
+![graph_balance](https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/cafd3725-003c-4873-a124-6810d496d66b)
+
+2. Swing up:
+[swing_up.webm](https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/6b9a9d66-65b0-4451-b2da-1e59e2cfc1bd)
+![graph 2 - swing up](https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/24006527-40cf-475a-bc8a-4237c8220786)

From 73a0a74f0a9b4efd96f3b75857910aee0c1ab666 Mon Sep 17 00:00:00 2001
From: Atharav1805 <168408064+Atharav1805@users.noreply.github.com>
Date: Tue, 11 Jun 2024 18:32:15 +0530
Subject: [PATCH 08/22] Update Results.md

---
 Results.md | 47 +++++++++++++++++++++++++++++++++++++++++------
 1 file changed, 41 insertions(+), 6 deletions(-)

diff --git a/Results.md b/Results.md
index bae0254..d7cd7f3 100644
--- a/Results.md
+++ b/Results.md
@@ -1,7 +1,42 @@
-1. Balance:
-[balance.webm](https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/9e45ce17-7df3-47fb-a1ae-0e2064d48eb7)
-![graph_balance](https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/cafd3725-003c-4873-a124-6810d496d66b)
+# Inverted Pendulum Problems
 
-2. Swing up:
-[swing_up.webm](https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/6b9a9d66-65b0-4451-b2da-1e59e2cfc1bd)
-![graph 2 - swing up](https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/24006527-40cf-475a-bc8a-4237c8220786)
+This repository contains solutions to various inverted pendulum problems. Below are two scenarios: Balancing and Swing Up. Each scenario includes a video demonstration and a corresponding performance graph.
+
+## 1. Balance
+
+### Video Demonstration
+[![Balance](https://img.youtube.com/vi/9e45ce17-7df3-47fb-a1ae-0e2064d48eb7/0.jpg)](https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/9e45ce17-7df3-47fb-a1ae-0e2064d48eb7)
+
+<video src="https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/9e45ce17-7df3-47fb-a1ae-0e2064d48eb7" controls="controls" style="max-width: 100%; height: auto;">
+    Your browser does not support the video tag.
+</video>
+
+### Performance Graph
+![Graph: Balance](https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/cafd3725-003c-4873-a124-6810d496d66b)
+
+## 2. Swing Up
+
+### Video Demonstration
+[![Swing Up](https://img.youtube.com/vi/6b9a9d66-65b0-4451-b2da-1e59e2cfc1bd/0.jpg)](https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/6b9a9d66-65b0-4451-b2da-1e59e2cfc1bd)
+
+<video src="https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/6b9a9d66-65b0-4451-b2da-1e59e2cfc1bd" controls="controls" style="max-width: 100%; height: auto;">
+    Your browser does not support the video tag.
+</video>
+
+### Performance Graph
+![Graph: Swing Up](https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/24006527-40cf-475a-bc8a-4237c8220786)
+
+## Getting Started
+
+To run these simulations, clone this repository and follow the instructions provided in the respective folders.
+
+### Prerequisites
+
+- Python 3.x
+- [Other dependencies]
+
+### Installation
+
+```bash
+git clone https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav.git
+cd Inverted-pendulum-problems-Atharav

From 8dc5850d92ee20a6bb85f7cdab0be05ddeb56636 Mon Sep 17 00:00:00 2001
From: Atharav1805 <168408064+Atharav1805@users.noreply.github.com>
Date: Tue, 11 Jun 2024 18:33:13 +0530
Subject: [PATCH 09/22] Update Results.md

---
 Results.md | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/Results.md b/Results.md
index d7cd7f3..b5f069c 100644
--- a/Results.md
+++ b/Results.md
@@ -7,9 +7,7 @@ This repository contains solutions to various inverted pendulum problems. Below
 ### Video Demonstration
 [![Balance](https://img.youtube.com/vi/9e45ce17-7df3-47fb-a1ae-0e2064d48eb7/0.jpg)](https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/9e45ce17-7df3-47fb-a1ae-0e2064d48eb7)
 
-<video src="https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/9e45ce17-7df3-47fb-a1ae-0e2064d48eb7" controls="controls" style="max-width: 100%; height: auto;">
-    Your browser does not support the video tag.
-</video>
+
 
 ### Performance Graph
 ![Graph: Balance](https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/cafd3725-003c-4873-a124-6810d496d66b)

From ac59d1508bcb3fd04daf260d4029e65972b96f27 Mon Sep 17 00:00:00 2001
From: Atharav1805 <168408064+Atharav1805@users.noreply.github.com>
Date: Tue, 11 Jun 2024 18:34:03 +0530
Subject: [PATCH 10/22] Update Results.md

---
 Results.md | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/Results.md b/Results.md
index b5f069c..16b4e23 100644
--- a/Results.md
+++ b/Results.md
@@ -5,9 +5,10 @@ This repository contains solutions to various inverted pendulum problems. Below
 ## 1. Balance
 
 ### Video Demonstration
-[![Balance](https://img.youtube.com/vi/9e45ce17-7df3-47fb-a1ae-0e2064d48eb7/0.jpg)](https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/9e45ce17-7df3-47fb-a1ae-0e2064d48eb7)
-
 
+<video src="https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/9e45ce17-7df3-47fb-a1ae-0e2064d48eb7" controls="controls" style="max-width: 100%; height: auto;">
+    Your browser does not support the video tag.
+</video>
 
 ### Performance Graph
 ![Graph: Balance](https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/cafd3725-003c-4873-a124-6810d496d66b)
@@ -15,7 +16,7 @@ This repository contains solutions to various inverted pendulum problems. Below
 ## 2. Swing Up
 
 ### Video Demonstration
-[![Swing Up](https://img.youtube.com/vi/6b9a9d66-65b0-4451-b2da-1e59e2cfc1bd/0.jpg)](https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/6b9a9d66-65b0-4451-b2da-1e59e2cfc1bd)
+
 
 <video src="https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/6b9a9d66-65b0-4451-b2da-1e59e2cfc1bd" controls="controls" style="max-width: 100%; height: auto;">
     Your browser does not support the video tag.

From 6a1c95edbd1456439f27248343a5e5f992407922 Mon Sep 17 00:00:00 2001
From: Atharav1805 <168408064+Atharav1805@users.noreply.github.com>
Date: Tue, 11 Jun 2024 18:34:31 +0530
Subject: [PATCH 11/22] Update Results.md

---
 Results.md | 13 -------------
 1 file changed, 13 deletions(-)

diff --git a/Results.md b/Results.md
index 16b4e23..017243a 100644
--- a/Results.md
+++ b/Results.md
@@ -25,17 +25,4 @@ This repository contains solutions to various inverted pendulum problems. Below
 ### Performance Graph
 ![Graph: Swing Up](https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/24006527-40cf-475a-bc8a-4237c8220786)
 
-## Getting Started
 
-To run these simulations, clone this repository and follow the instructions provided in the respective folders.
-
-### Prerequisites
-
-- Python 3.x
-- [Other dependencies]
-
-### Installation
-
-```bash
-git clone https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav.git
-cd Inverted-pendulum-problems-Atharav

From 2bae2c83972bf29d53d1180ffb64426a9f86fa6e Mon Sep 17 00:00:00 2001
From: Atharav1805 <168408064+Atharav1805@users.noreply.github.com>
Date: Wed, 12 Jun 2024 14:31:17 +0530
Subject: [PATCH 12/22] Rename README.md to Assignment.md

---
 README.md => Assignment.md | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 rename README.md => Assignment.md (100%)

diff --git a/README.md b/Assignment.md
similarity index 100%
rename from README.md
rename to Assignment.md

From 7bf9e1bfde9dabd259c734282c82995e3ac293cb Mon Sep 17 00:00:00 2001
From: Atharav1805 <168408064+Atharav1805@users.noreply.github.com>
Date: Wed, 12 Jun 2024 14:31:44 +0530
Subject: [PATCH 13/22] Rename Results.md to README.md

---
 Results.md => README.md | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 rename Results.md => README.md (100%)

diff --git a/Results.md b/README.md
similarity index 100%
rename from Results.md
rename to README.md

From 91d63676ea592163d328a3b07d38f13bf95a9243 Mon Sep 17 00:00:00 2001
From: Atharav1805 <168408064+Atharav1805@users.noreply.github.com>
Date: Wed, 12 Jun 2024 14:32:25 +0530
Subject: [PATCH 14/22] Update README.md

---
 README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/README.md b/README.md
index 017243a..121985b 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,6 @@
 # Inverted Pendulum Problems
 
-This repository contains solutions to various inverted pendulum problems. Below are two scenarios: Balancing and Swing Up. Each scenario includes a video demonstration and a corresponding performance graph.
+This repository contains solutions to the inverted pendulum problems.Each scenario includes a video demonstration and a corresponding performance graph.
 
 ## 1. Balance
 

From 8349969787a5dd75c278e9fdd46a7e1e6c014069 Mon Sep 17 00:00:00 2001
From: Atharav1805 <168408064+Atharav1805@users.noreply.github.com>
Date: Thu, 13 Jun 2024 03:11:16 +0530
Subject: [PATCH 15/22] Add files via upload

---
 double_inverted/config/config.rviz            | 130 +++++++++++++
 double_inverted/double_inverted/__init__.py   |   0
 .../__pycache__/__init__.cpython-310.pyc      | Bin 0 -> 173 bytes
 .../dynamics_double.cpython-310.pyc           | Bin 0 -> 4332 bytes
 .../double_inverted/dynamics_double.py        | 172 ++++++++++++++++++
 .../launch/double_inverted_pedulum.launch.py  |  28 +++
 double_inverted/package.xml                   |  20 ++
 double_inverted/resource/double_inverted      |   0
 double_inverted/setup.cfg                     |   4 +
 double_inverted/setup.py                      |  32 ++++
 double_inverted/test/test_copyright.py        |  25 +++
 double_inverted/test/test_flake8.py           |  25 +++
 double_inverted/test/test_pep257.py           |  23 +++
 13 files changed, 459 insertions(+)
 create mode 100644 double_inverted/config/config.rviz
 create mode 100644 double_inverted/double_inverted/__init__.py
 create mode 100644 double_inverted/double_inverted/__pycache__/__init__.cpython-310.pyc
 create mode 100644 double_inverted/double_inverted/__pycache__/dynamics_double.cpython-310.pyc
 create mode 100644 double_inverted/double_inverted/dynamics_double.py
 create mode 100644 double_inverted/launch/double_inverted_pedulum.launch.py
 create mode 100644 double_inverted/package.xml
 create mode 100644 double_inverted/resource/double_inverted
 create mode 100644 double_inverted/setup.cfg
 create mode 100644 double_inverted/setup.py
 create mode 100644 double_inverted/test/test_copyright.py
 create mode 100644 double_inverted/test/test_flake8.py
 create mode 100644 double_inverted/test/test_pep257.py

diff --git a/double_inverted/config/config.rviz b/double_inverted/config/config.rviz
new file mode 100644
index 0000000..f59fc28
--- /dev/null
+++ b/double_inverted/config/config.rviz
@@ -0,0 +1,130 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 617
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz_default_plugins/Marker
+      Enabled: true
+      Name: Marker
+      Namespaces:
+        "": true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /pendulum_viz
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Angle: 0
+      Class: rviz_default_plugins/TopDownOrtho
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Scale: 142.98838806152344
+      Target Frame: <Fixed Frame>
+      Value: TopDownOrtho (rviz_default_plugins)
+      X: 0
+      Y: 0
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1200
+  X: 72
+  Y: 60
diff --git a/double_inverted/double_inverted/__init__.py b/double_inverted/double_inverted/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/double_inverted/double_inverted/__pycache__/__init__.cpython-310.pyc b/double_inverted/double_inverted/__pycache__/__init__.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..2a2d37cc3f0dff898e8e57633f385f11ecba3342
GIT binary patch
literal 173
zcmd1j<>g`kf|c`P(?IlN5P=LBfgA@QE@lA|DGb33nv8xc8Hzx{2;x_Oenx(7s(xZg
zMq*K7nZ8SEadt_5fqp@1UP@_BX|8@!X=YA}eoB66Qch}oW?orpQAsK$KOQKRSrQ+w
YS5SG2!zMQ$s5jLPWK%H{kYHf|08Jq*DF6Tf

literal 0
HcmV?d00001

diff --git a/double_inverted/double_inverted/__pycache__/dynamics_double.cpython-310.pyc b/double_inverted/double_inverted/__pycache__/dynamics_double.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..5f3b91f52e74e4de57ba5ecb7583c14c7b3eff98
GIT binary patch
literal 4332
zcmZ`+TaP1074GVr+jq~5*W=9OGMf;<Kz6bdNI*g)Aq!|F7%glhBonFB<7#_)w%hGg
zw>KH9EhIK5Py7IQfEmF{{z0C3&mX8K1P^Ex3Gsl01PaS{s@vY#+1+l}sXC|5sj5D8
zZcdubnt|uvM<?S~t{BGO2)X#!K;A-2{{$cm!6IYGbZ^Gi&}IeA89JbuksDWrmDn44
zCA~VVf^J2%xIV0FnjJOb=CG;9PSlFq!?qr~Q77IQZs>6(+Kjh`TXA>TWyX&T;fd-;
zhNuShd)DxZXo{Msf5e8pU|TfqnDSQ8yJtX--PUc;x{s3@hXYetX*gBZC`kuQ*@uY;
z(0e!L($B(Vin03>U)~8MM$WAyoMsTIznjSS=fTbCY@R9icIIb6I->P1K30jO|G*&7
z`WTvlbz%#)YYd(B$Q9<Wg60tgt$J2Fhc@~+=dRHwPR+XwoH)8h&!Aq<JXyjGT;h0w
zgez?9We+{<R~OE(fz}l6u!X*j))AHAhVX(7oN!ZAhg%;TW2QCf4ZETtnjaa^ZA-M#
z_o{~Ihz*Rk#irOo-~ZUKjo~x5jX_uSMKV8%0v=B91~Ll-p9NDfkLK}x6YBW;N>SzD
zFK<frko?a#ColZ^^PhhD`<LDd{{{Men!A4hFrv}wp|OibicbFwkQw)kGbY$MV@7VA
znZnGC6Kfaa6Z;%v?ttoM6=4bU+yd@tyejMxuK}-TPzofVmUA0Ct*m|4KF1ooQ;bBV
zPZ-n$xCzcJ^j-8<(D%@9qwk}C2K^3J+&wqWo)y*dR0URh1gjG!3@EN~sM@9GRtl4C
zeA=S9zry+5aIt9br(|_}90cOXAKk%xrfm`mhf&^lATxR8oo7G)>+3s(ZWn_a`bPF{
zmN&9@tGtoDDU5zEoQ7HGN8ut6i}u^2Q83HG=>*(ZZn1R>ml>wVMEPO4<={Exr1M!I
zmB+Dbm~pNeBN@=KvM>g{SDbM^6Z)t!fX_@vr>a*{)?cLgQngjm3$$FMQ!Y6xP7p{*
zcdlx8!*s5XD`k}kjg=}mIMr4umCldSkqq%I$y9p8x7tMOi5E&!j>eQt=T?=B;{+0(
zo$;9r?kYDqI^m&EUUnR0{&oH;X7s3}9IPn^s$TMlBzslW9wOz$0lzSz5mlQ68IO|5
z1RAr$={TV{lm^lG6Q-)1k0L)!Ise4?&(HUdlQ`J-vtwWSclU1u>76W@t?gp}XdXsl
z|B~N5j-O8bI2@%o2s!Ar*{SS8FR!7c&jU1A&+6D6)@3!T#a!lD9;3Hqw%8_HT)njZ
z+OMyzj9|11d|1rbK%@J52Q58>$HFhDF*NO+=<Tudp0l);_R?9p%Sz7PvzOkox~whh
zc{TU?#<G!HIl@`)bdB65m^pdl9z1=^=psZeo4K1i@bhNZShfm|yjIL+E^)3Tw0s4a
zn^!MVv9_JNn5{o51$wb_A>)w@yuZ0?EZezVH=ZO@oDFL|Cb^nfuS(x@>6}>oqMSY@
zXf5p6$tq`FUd!uwJMT>FbEL9?c_`~p?Vzfh7<O{K=nj+_T}L}mY$EA8Bn4e%PG`#T
zrSwk+8}b#Run!fRD>DXjd?3F`oZlkwI)N_{_%ea768IW`azqeizH&t{O(0eD7pxxE
z{m{~Z$X7{JgX<*E#)s%qBD}~N%ro}@pErBVe9$w^|7}|47p7%@vFGf7vU7p5=`41h
zYI@^G@&XkjYn%HOkjywUD1x6b*a}-=oMq-2vcL(V{0T*T7p+2iU7EQmSdP0!^!9SH
z)9Sg|HMWhV1q`+eDoJYU8wT;eN4bI;R^nKRSHOitUdDCYQxhi{)a484<W&N15coEM
zCumxJ9}}uk2KBpsG!N9~I_9mtZeSM|X^KpKha_4bA{J+Xi~|uO>R!^n+y>7PTG|0<
zu^QVkcUX^gP0zI1;&QN9Z*%R*5&IFflgdf;+SPa9>XeR`aMGNeFkU!2<90^MX52*&
zSLdiiFkd~X@w%`uYVc;(f}APr+~iFS+ZuK>oz%XO>9kAEm|MI_l)8~o3f?MdBw-<8
z(@Gef5YH7Y)gumacgR^MZA~XhO^2h|oMdg@!9~LnCpZ(|)@SVVXUcvj+r_#L$^?f$
zn>`2V=LLetJqG-RoW5=kw5w+?=4B~jqYM$1Hw@F@mQM5Dp|WCsCR@<g<afWN=aV;O
z1DLLZCKwpOXP>>TbN}F_BIUWqfiEcCj-?-?vJ&z|uqhLnsrKWWhd<!A-~HLmTS#K3
zvp`vI-?*V%e?*B-*|Q`KR6UVlFh$uwB=!Na_sEX|Z8ow(E3s3>7DW^=ffr_}!-ozE
z6bh6(qZ&9>&M1izsTh8~q-q0obTCpDCe`*Md7pABs)R5RsxtMbILSzPQ8*5Ck+4Ie
zl`^>x+`^jM>&hV3wKo`OSAKmpI9Lr{Q@g8DohRe9z4Aitvi0A<W(%~Gybh_Yd~4`k
z(?znwYi=U-QQv2M^bL3hX7P43wrMWD{?s`1xPOjFk*kNsq$=$peGD?x^cmAuW!}S!
zuwiQZunzD4_rZe)auZAgi)`gXfN&~FbYK=*vf^}d`3s-J{2$O#^29EKPk-a7r!9T@
zVj(oxp<)gNLV#kf#(A8eW~Xt3^Y>A16*D?=DRUOem#~X`nZQ>F3;>iX{i#S|WmB&Y
zEU9#tRACf}<zb%b5$zWwwYe_e#;ZC}*Cjd*>aZp0_9T6vYL;cuD2O7?FX-SJu~4Fs
z^naBK=ae?{jG9%0GHlj&cUT`G&qlbjZQCMR@p_=)vUk0%+4Sn2m-Pyd1UR}r@&<i<
zl)(_fkP8I+4XXFdq+o~Kf`FlGdv<11^asVckOio=^W`KRxXO{EXm&~^Uzq95Q1#95
zSF|C5G?U3Gr?Omm>G3=h$p=&1`V4!j>e@Fhe7DZ6cI=1KKY}Nvk8i+SVc2Ga$qz^l
z_e%4e57^<Lubk;TM(Ia6NNvP^Hj9!h3Xf0@(g<!|^wuoObjng#Jv-KQL{U+^Pm(qL
z4cF+Tk}OX3s0hU$haw4${ZWu!BQw<zqPOx79fCOM#Uy18wJ`}$-DdLiAzcTh_Gq4F
zN&JvXRlF|wwbJy*$z!6?m0SQ?C*F4p6aF6kSxol;JZQ#)Gm~Z-)L6&tn8q(_S3D0k
G^5Fkaxk?TI

literal 0
HcmV?d00001

diff --git a/double_inverted/double_inverted/dynamics_double.py b/double_inverted/double_inverted/dynamics_double.py
new file mode 100644
index 0000000..b5450d2
--- /dev/null
+++ b/double_inverted/double_inverted/dynamics_double.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):
+
+    # Initialize State/s : at theta = pi/2, pi/2
+    theta1_0 = np.pi / 2
+    theta2_0 = np.pi / 2
+    theta1_dot0 = 0.0
+    theta2_dot0 = 0.0
+    
+    # You can use similar variations as the original code for initialization
+
+    
+    #theta is almost upright (randomized)
+    theta1_0 = np.pi - (np.random.rand() - 0.5) / 2
+    theta1_0 = (theta1_0 + np.pi)%(2*np.pi) - np.pi
+
+    theta2_0 = np.pi - (np.random.rand() - 0.5) / 2
+    theta2_0 = (theta2_0 + np.pi)%(2*np.pi) - np.pi
+
+    theta1_dot0 = 0
+    theta2_dot0 = 0
+    
+    
+    # Input/s
+    torque_value = 0.0
+
+    # Param/s
+    mass1 = 1.0
+    mass2 = 1.0
+    g = 9.81
+    l1 = 1.0
+    l2 = 1.0
+    state_update_frequency = 500
+    state_update_timeperiod = 1 / state_update_frequency
+
+    feedback_frequency = 50
+
+    def __init__(self):
+        super().__init__('double_pendulum')
+
+        # Timers
+        self.update_states_timer = self.create_timer(1 / self.state_update_frequency, self.update_pendulum_states)
+        #self.feedback_timer = self.create_timer(1 / self.feedback_frequency, self.feedback)
+
+        # Publishers/Subscribers/Services
+        self.visualizer = self.create_publisher(Marker, '/pendulum_viz', 1)
+        self.feedback_pub = self.create_publisher(States, '/state_feedback', 1)
+        self.input = self.create_subscription(TorqueInput, '/torque_input', self.update_input_torque, 5)
+
+        # Attributes
+        self.t_start = time.time()
+        self.t_prev = time.time() - 0.0001
+        self.obj_id = 0
+
+        # States
+        self.theta1 = self.theta1_0
+        self.theta2 = self.theta2_0
+        self.theta1_dot = self.theta1_dot0
+        self.theta2_dot = self.theta2_dot0
+
+        self.get_logger().info('Double Inverted Pendulum node initialized')
+        self.get_logger().info('Accepting Input')
+        self.get_logger().info('Publishing Feedback')
+
+    def f(self, x, u):
+        m1, m2, l1, l2, g = self.mass1, self.mass2, self.l1, self.l2, self.g
+        theta1, theta2, theta1_dot, theta2_dot = x
+
+        delta = theta2 - theta1
+        denom1 = (m1 + m2) * l1 - m2 * l1 * cos(delta) ** 2
+        denom2 = (l2 / l1) * denom1
+
+        theta1_ddot = (m2 * l1 * theta1_dot ** 2 * sin(delta) * cos(delta) +
+                       m2 * g * sin(theta2) * cos(delta) +
+                       m2 * l2 * theta2_dot ** 2 * sin(delta) -
+                       (m1 + m2) * g * sin(theta1)) / denom1
+
+        theta2_ddot = (-m2 * l2 * theta2_dot ** 2 * sin(delta) * cos(delta) +
+                       (m1 + m2) * g * sin(theta1) * cos(delta) -
+                       (m1 + m2) * l1 * theta1_dot ** 2 * sin(delta) -
+                       (m1 + m2) * g * sin(theta2)) / denom2
+
+        return np.array([theta1_dot, theta2_dot, theta1_ddot, theta2_ddot])
+
+    def update_pendulum_states(self):
+        dt = time.time() - self.t_prev
+        self.t_prev = time.time()
+
+        x = np.array([self.theta1, self.theta2, self.theta1_dot, self.theta2_dot])
+        x_intermediate = x + 0.5 * dt * self.f(x, self.torque_value)
+        x += dt * self.f(x_intermediate, self.torque_value)
+
+        self.theta1, self.theta2, self.theta1_dot, self.theta2_dot = x
+
+        self.visualize_pendulum()
+        return
+
+    '''
+    def feedback(self):
+        states_msg = States()
+        states_msg.theta1 = self.theta1
+        states_msg.theta2 = self.theta2
+        states_msg.theta1_dot = self.theta1_dot
+        states_msg.theta2_dot = self.theta2_dot
+        self.feedback_pub.publish(states_msg)
+        return
+    '''
+    
+    def visualize_pendulum(self):
+        pendulum_marker = Marker()
+        pendulum_marker.header.frame_id = "map"
+        pendulum_marker.id = self.obj_id
+        pendulum_marker.type = Marker.LINE_STRIP
+        pendulum_marker.action = Marker.ADD
+        pendulum_marker.pose.orientation.w = 1.0
+        pendulum_marker.scale.x = 0.05  # Line width
+
+        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
+
+        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_marker.points = [point_1, point_2, point_3]
+
+        pendulum_marker.color.r = 1.0
+        pendulum_marker.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_marker.lifetime = Duration_of_pendulum_marker
+
+        self.visualizer.publish(pendulum_marker)
+
+        self.obj_id += 1
+
+    def update_input_torque(self, msg):
+        self.torque_value = max(-5, min(5, msg.torque_value))
+        return
+
+def main(args=None):
+    rclpy.init(args=args)
+    pendulum_ = double_inverted_pendulum()
+    rclpy.spin(pendulum_)
+
+    pendulum_.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
diff --git a/double_inverted/launch/double_inverted_pedulum.launch.py b/double_inverted/launch/double_inverted_pedulum.launch.py
new file mode 100644
index 0000000..20bc0fb
--- /dev/null
+++ b/double_inverted/launch/double_inverted_pedulum.launch.py
@@ -0,0 +1,28 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+import os
+
+def generate_launch_description():
+
+    package_dir = os.path.dirname(os.path.abspath(__file__))
+    rviz_config_file = os.path.join(package_dir, '..', 'config', 'config.rviz')
+    
+    visualizer = Node(
+        package = 'rviz2',
+        executable = 'rviz2',
+        # name='rviz2',
+        arguments = ['-d', rviz_config_file]
+
+    )
+
+    dynamics = Node(
+        package = 'double_inverted',
+        executable = 'dynamics_double'
+    )
+
+    return LaunchDescription([
+        visualizer,
+        dynamics
+    ])
\ No newline at end of file
diff --git a/double_inverted/package.xml b/double_inverted/package.xml
new file mode 100644
index 0000000..bbf1c1d
--- /dev/null
+++ b/double_inverted/package.xml
@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>double_inverted</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="atharav.sonawane.aryo@gmail.com">atharav</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>rclpy</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
diff --git a/double_inverted/resource/double_inverted b/double_inverted/resource/double_inverted
new file mode 100644
index 0000000..e69de29
diff --git a/double_inverted/setup.cfg b/double_inverted/setup.cfg
new file mode 100644
index 0000000..2daf2b6
--- /dev/null
+++ b/double_inverted/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/double_inverted
+[install]
+install_scripts=$base/lib/double_inverted
diff --git a/double_inverted/setup.py b/double_inverted/setup.py
new file mode 100644
index 0000000..20d03f2
--- /dev/null
+++ b/double_inverted/setup.py
@@ -0,0 +1,32 @@
+from setuptools import find_packages, setup
+import os
+from glob import glob
+
+package_name = 'double_inverted'
+
+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']),
+        (os.path.join('share', package_name, 'launch/'),
+         glob('launch/*launch.[pxy][yma]*')),
+         (os.path.join('share', package_name, 'config/'),
+         glob('config/**'))
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='atharav',
+    maintainer_email='atharav.sonawane.aryo@gmail.com',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'dynamics_double = double_inverted.dynamics_double:main'
+        ],
+    },
+)
diff --git a/double_inverted/test/test_copyright.py b/double_inverted/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/double_inverted/test/test_copyright.py
@@ -0,0 +1,25 @@
+# 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
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'
diff --git a/double_inverted/test/test_flake8.py b/double_inverted/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/double_inverted/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/test/test_pep257.py b/double_inverted/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/double_inverted/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'

From c56e28c90c58e0cabe9fe57ef13c8133b2a0da1f Mon Sep 17 00:00:00 2001
From: Atharav1805 <168408064+Atharav1805@users.noreply.github.com>
Date: Thu, 13 Jun 2024 03:12:49 +0530
Subject: [PATCH 16/22] Update dynamics_double.py

---
 double_inverted/double_inverted/dynamics_double.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/double_inverted/double_inverted/dynamics_double.py b/double_inverted/double_inverted/dynamics_double.py
index b5450d2..f800ea8 100644
--- a/double_inverted/double_inverted/dynamics_double.py
+++ b/double_inverted/double_inverted/dynamics_double.py
@@ -22,7 +22,7 @@ class double_inverted_pendulum(Node):
     
     # You can use similar variations as the original code for initialization
 
-    
+    '''
     #theta is almost upright (randomized)
     theta1_0 = np.pi - (np.random.rand() - 0.5) / 2
     theta1_0 = (theta1_0 + np.pi)%(2*np.pi) - np.pi
@@ -32,7 +32,7 @@ class double_inverted_pendulum(Node):
 
     theta1_dot0 = 0
     theta2_dot0 = 0
-    
+    '''
     
     # Input/s
     torque_value = 0.0

From 0c6f5021398ee3c635fd52c08db107bc2c6a9f5d Mon Sep 17 00:00:00 2001
From: Atharav1805 <168408064+Atharav1805@users.noreply.github.com>
Date: Thu, 13 Jun 2024 03:19:29 +0530
Subject: [PATCH 17/22] Update README.md

---
 README.md | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/README.md b/README.md
index 121985b..45ac2e2 100644
--- a/README.md
+++ b/README.md
@@ -2,6 +2,8 @@
 
 This repository contains solutions to the inverted pendulum problems.Each scenario includes a video demonstration and a corresponding performance graph.
 
+# Single Pendulum
+
 ## 1. Balance
 
 ### Video Demonstration
@@ -26,3 +28,7 @@ This repository contains solutions to the inverted pendulum problems.Each scenar
 ![Graph: Swing Up](https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/24006527-40cf-475a-bc8a-4237c8220786)
 
 
+# Double Pendulum
+
+<video src="https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/5fb8181c-76e9-472e-85c9-073f20962a18)" controls="controls" style="max-width: 100%; height: auto;">
+

From 664b0852c3cacc8be7e0e9ae5e3d3b65d298a8e3 Mon Sep 17 00:00:00 2001
From: Atharav1805 <168408064+Atharav1805@users.noreply.github.com>
Date: Thu, 13 Jun 2024 03:20:54 +0530
Subject: [PATCH 18/22] Update README.md

---
 README.md | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/README.md b/README.md
index 45ac2e2..737d940 100644
--- a/README.md
+++ b/README.md
@@ -31,4 +31,6 @@ This repository contains solutions to the inverted pendulum problems.Each scenar
 # Double Pendulum
 
 <video src="https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/5fb8181c-76e9-472e-85c9-073f20962a18)" controls="controls" style="max-width: 100%; height: auto;">
+    Your browser does not support the video tag.
+</video>
 

From ee6aad8e9300a0d8023a1453c75b932943d0ed9b Mon Sep 17 00:00:00 2001
From: Atharav1805 <168408064+Atharav1805@users.noreply.github.com>
Date: Thu, 13 Jun 2024 03:23:03 +0530
Subject: [PATCH 19/22] Update README.md

---
 README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/README.md b/README.md
index 737d940..a8d2aa8 100644
--- a/README.md
+++ b/README.md
@@ -30,7 +30,7 @@ This repository contains solutions to the inverted pendulum problems.Each scenar
 
 # Double Pendulum
 
-<video src="https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/5fb8181c-76e9-472e-85c9-073f20962a18)" controls="controls" style="max-width: 100%; height: auto;">
+<video src="https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/24b4e456-98b5-4f03-95af-e329b60c5233)" controls="controls" style="max-width: 100%; height: auto;">
     Your browser does not support the video tag.
 </video>
 

From a59e276040792f7987935780b6607ca2472998ca Mon Sep 17 00:00:00 2001
From: Atharav1805 <168408064+Atharav1805@users.noreply.github.com>
Date: Thu, 13 Jun 2024 03:25:01 +0530
Subject: [PATCH 20/22] Update README.md

---
 README.md | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/README.md b/README.md
index a8d2aa8..51813dd 100644
--- a/README.md
+++ b/README.md
@@ -30,7 +30,6 @@ This repository contains solutions to the inverted pendulum problems.Each scenar
 
 # Double Pendulum
 
-<video src="https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/24b4e456-98b5-4f03-95af-e329b60c5233)" controls="controls" style="max-width: 100%; height: auto;">
-    Your browser does not support the video tag.
-</video>
+[double inverted pendulum.webm](https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/1aea4261-4329-4b0c-b2e6-fa9eaaf0254e)
+
 

From 011e4e894ad5addfd8d387d01f5ca01583a39b4c Mon Sep 17 00:00:00 2001
From: Atharav1805 <168408064+Atharav1805@users.noreply.github.com>
Date: Sun, 16 Jun 2024 01:37:44 +0530
Subject: [PATCH 21/22] Minor changes

---
 controller_node_swing_up.py | 12 +++++++-----
 1 file changed, 7 insertions(+), 5 deletions(-)

diff --git a/controller_node_swing_up.py b/controller_node_swing_up.py
index eaa739a..97c685b 100644
--- a/controller_node_swing_up.py
+++ b/controller_node_swing_up.py
@@ -54,7 +54,7 @@ def pose_callback(self, msg: States):
         
         if abs(self.theta) >= np.pi -0.4 :
             self.swing_up_complete = True
-            self.get_logger().info("Swing up complete")
+            self.get_logger().info("Swing up complete, now balancing the pendulum...")
 
         if self.swing_up_complete == True:
             self.PID_balance()
@@ -65,13 +65,15 @@ def pose_callback(self, msg: States):
 
     def swing_up(self):
 
-        if abs(self.theta - self.previous_theta) >= 0.4:
+        if abs(self.theta - self.previous_theta) >= 0.6:
             self.torque = 0.0
 
         elif abs(self.theta) < np.pi / 1.5:
             if abs(self.theta - self.previous_theta) < 0.05:
                 self.previous_torque *= -1
                 self.torque = self.previous_torque
+        #else:
+            #self.torque = self.previous_torque
         
         self.send_torque_info()
         
@@ -91,7 +93,7 @@ def PID_balance(self):
 
         # Set the PID gains
         Kp = 50.0
-        Ki = 0.01
+        Ki = 0.001
         Kd = 6.0
 
         if self.theta < 0:
@@ -108,10 +110,10 @@ def PID_balance(self):
         self.previous_error = error
 
         p = Kp * error
-        #i = Ki * self.integral
+        i = Ki * self.integral
         d = Kd * derivative
 
-        self.torque = p + d 
+        self.torque = p + i + d 
         self.send_torque_info()
 
 

From b42d5bdd20b12594d28e7779c4276d670180d1e3 Mon Sep 17 00:00:00 2001
From: Atharav1805 <168408064+Atharav1805@users.noreply.github.com>
Date: Sun, 16 Jun 2024 01:40:39 +0530
Subject: [PATCH 22/22] Minor changes

---
 .../dynamics_double.cpython-310.pyc           | Bin 4332 -> 4332 bytes
 .../double_inverted/dynamics_double.py        |   4 ++--
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/double_inverted/double_inverted/__pycache__/dynamics_double.cpython-310.pyc b/double_inverted/double_inverted/__pycache__/dynamics_double.cpython-310.pyc
index 5f3b91f52e74e4de57ba5ecb7583c14c7b3eff98..5078e8a11cd0b25e1d01b63d40b30ffd4d3c7293 100644
GIT binary patch
delta 20
acmaE(_(qXCpO=@50SLT>vNv)+6aWA^4+Sm&

delta 20
acmaE(_(qXCpO=@50SLY)Wo_hsC;$LJo(3rZ

diff --git a/double_inverted/double_inverted/dynamics_double.py b/double_inverted/double_inverted/dynamics_double.py
index f800ea8..b5450d2 100644
--- a/double_inverted/double_inverted/dynamics_double.py
+++ b/double_inverted/double_inverted/dynamics_double.py
@@ -22,7 +22,7 @@ class double_inverted_pendulum(Node):
     
     # You can use similar variations as the original code for initialization
 
-    '''
+    
     #theta is almost upright (randomized)
     theta1_0 = np.pi - (np.random.rand() - 0.5) / 2
     theta1_0 = (theta1_0 + np.pi)%(2*np.pi) - np.pi
@@ -32,7 +32,7 @@ class double_inverted_pendulum(Node):
 
     theta1_dot0 = 0
     theta2_dot0 = 0
-    '''
+    
     
     # Input/s
     torque_value = 0.0