For both parts of the assignment

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

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
+}

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"
+    ]
+}

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()

double_inverted_pendulum/package.xml

@@ -0,0 +1,32 @@
+<?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_pendulum</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="parthgoel9@gmail.com">parthgoel</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>turtlesim</depend>
+  <depend>numpy</depend>
+  <depend>math</depend>
+  <depend>sensor_msgs</depend>
+  <depend>eufs_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>builtin_interfaces_msgs</depend>
+  <depend>message_filters</depend>
+  <depend>custom_msgs</depend>
+  <depend>time</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>

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

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"
+        ],
+    },
+)

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'

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)

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'

single_inverted/launch/single_inverted_pendulum.launch.py

@@ -22,7 +22,10 @@ def generate_launch_description():
         executable = 'dynamics_sim'
     )
 
+
+
+
     return LaunchDescription([
         visualizer,
         dynamics
-    ])
+    ])

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"
         ],
     },
 )

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