Completed the assignment and uploaded videos

Assignment.md

@@ -0,0 +1,33 @@
+# Inverted Pendulum
+#### Implementation of control laws on simulated inverted pendulum on ros2
+
+![Output sample](illustrations/screen-capture.gif)
+
+## Exercises
+![Output sample](illustrations/diagram.gif)
+#### 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
+- **Swing-up** [Initial state - downward point at stable equilibrium] Write a controller to first swing up the pendulum, then balance on top.
+
+#### 2) Double Inverted Pendulum
+- Create a new pacakge with dynamics of an double inverted pendulum
+
+## How-to
+1. Clone this repository in /src folder of your ros2 workspace
+2. Build and source the workspace. Navigate to your workspace directory and run
+```
+colcon build --symlink-install
+source install/setup.bash
+```
+3. You can now launch the simulation
+```
+ros2 launch single_inverted single_inverted_pendulum.launch.py
+```
+4. Create a seperate 'controller' node which uses feedback to determine the torque value to accomplish tasks
+
+5. Change the initial states by varying the values theta0 and theta_dot0 manually or by un-commenting relevant sections for random initializations
+6. Custom messages definitions are used for input and feedback 
+
+
+

README.md

@@ -1,33 +1,35 @@
-# Inverted Pendulum
-#### Implementation of control laws on simulated inverted pendulum on ros2
-
-![Output sample](illustrations/screen-capture.gif)
-
-## Exercises
-![Output sample](illustrations/diagram.gif)
-#### 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
-- **Swing-up** [Initial state - downward point at stable equilibrium] Write a controller to first swing up the pendulum, then balance on top.
-
-#### 2) Double Inverted Pendulum
-- Create a new pacakge with dynamics of an double inverted pendulum
-
-## How-to
-1. Clone this repository in /src folder of your ros2 workspace
-2. Build and source the workspace. Navigate to your workspace directory and run
-```
-colcon build --symlink-install
-source install/setup.bash
-```
-3. You can now launch the simulation
-```
-ros2 launch single_inverted single_inverted_pendulum.launch.py
-```
-4. Create a seperate 'controller' node which uses feedback to determine the torque value to accomplish tasks
-
-5. Change the initial states by varying the values theta0 and theta_dot0 manually or by un-commenting relevant sections for random initializations
-6. Custom messages definitions are used for input and feedback 
+# Inverted Pendulum Problems
 
+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
+
+<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
+
+
+<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)
+
+
+# Double Pendulum
+
+[double inverted pendulum.webm](https://github.com/Atharav1805/Inverted-pendulum-problems-Atharav/assets/168408064/1aea4261-4329-4b0c-b2e6-fa9eaaf0254e)
 
 

controller_node.py

@@ -0,0 +1,118 @@
+#!/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.node_start_time = time.time()
+        self.prev_time = time.time()
+        self.previous_error = self.target_angle - self.theta
+        self.integral = 0.0
+
+        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.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.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
+
+        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 = 50.0
+        Ki = 0.001
+        Kd = 6.0
+
+        # 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 # calculate the torque value
+        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()
+
+
+
+

controller_node_swing_up.py

@@ -0,0 +1,150 @@
+#!/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.4 :
+            self.swing_up_complete = True
+            self.get_logger().info("Swing up complete, now balancing the pendulum...")
+
+        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.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()
+
+
+    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.001
+        Kd = 6.0
+
+        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()
+
+
+
+