├── .gitignore ├── LICENSE ├── README.md └── Week1 ├── .ipynb_checkpoints ├── task-checkpoint.ipynb └── tester-checkpoint.py ├── __pycache__ └── tester.cpython-312.pyc ├── numpy_basics.ipynb ├── task.ipynb └── tester.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | # Virtual Env 35 | .venv 36 | 37 | # Text files 38 | *.txt 39 | 40 | # Solutions 41 | solution*.ipynb -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Creative Commons Legal Code 2 | 3 | CC0 1.0 Universal 4 | 5 | CREATIVE COMMONS CORPORATION IS NOT A LAW FIRM AND DOES NOT PROVIDE 6 | LEGAL SERVICES. DISTRIBUTION OF THIS DOCUMENT DOES NOT CREATE AN 7 | ATTORNEY-CLIENT RELATIONSHIP. CREATIVE COMMONS PROVIDES THIS 8 | INFORMATION ON AN "AS-IS" BASIS. CREATIVE COMMONS MAKES NO WARRANTIES 9 | REGARDING THE USE OF THIS DOCUMENT OR THE INFORMATION OR WORKS 10 | PROVIDED HEREUNDER, AND DISCLAIMS LIABILITY FOR DAMAGES RESULTING FROM 11 | THE USE OF THIS DOCUMENT OR THE INFORMATION OR WORKS PROVIDED 12 | HEREUNDER. 13 | 14 | Statement of Purpose 15 | 16 | The laws of most jurisdictions throughout the world automatically confer 17 | exclusive Copyright and Related Rights (defined below) upon the creator 18 | and subsequent owner(s) (each and all, an "owner") of an original work of 19 | authorship and/or a database (each, a "Work"). 20 | 21 | Certain owners wish to permanently relinquish those rights to a Work for 22 | the purpose of contributing to a commons of creative, cultural and 23 | scientific works ("Commons") that the public can reliably and without fear 24 | of later claims of infringement build upon, modify, incorporate in other 25 | works, reuse and redistribute as freely as possible in any form whatsoever 26 | and for any purposes, including without limitation commercial purposes. 27 | These owners may contribute to the Commons to promote the ideal of a free 28 | culture and the further production of creative, cultural and scientific 29 | works, or to gain reputation or greater distribution for their Work in 30 | part through the use and efforts of others. 31 | 32 | For these and/or other purposes and motivations, and without any 33 | expectation of additional consideration or compensation, the person 34 | associating CC0 with a Work (the "Affirmer"), to the extent that he or she 35 | is an owner of Copyright and Related Rights in the Work, voluntarily 36 | elects to apply CC0 to the Work and publicly distribute the Work under its 37 | terms, with knowledge of his or her Copyright and Related Rights in the 38 | Work and the meaning and intended legal effect of CC0 on those rights. 39 | 40 | 1. Copyright and Related Rights. A Work made available under CC0 may be 41 | protected by copyright and related or neighboring rights ("Copyright and 42 | Related Rights"). Copyright and Related Rights include, but are not 43 | limited to, the following: 44 | 45 | i. the right to reproduce, adapt, distribute, perform, display, 46 | communicate, and translate a Work; 47 | ii. moral rights retained by the original author(s) and/or performer(s); 48 | iii. publicity and privacy rights pertaining to a person's image or 49 | likeness depicted in a Work; 50 | iv. rights protecting against unfair competition in regards to a Work, 51 | subject to the limitations in paragraph 4(a), below; 52 | v. rights protecting the extraction, dissemination, use and reuse of data 53 | in a Work; 54 | vi. database rights (such as those arising under Directive 96/9/EC of the 55 | European Parliament and of the Council of 11 March 1996 on the legal 56 | protection of databases, and under any national implementation 57 | thereof, including any amended or successor version of such 58 | directive); and 59 | vii. other similar, equivalent or corresponding rights throughout the 60 | world based on applicable law or treaty, and any national 61 | implementations thereof. 62 | 63 | 2. Waiver. To the greatest extent permitted by, but not in contravention 64 | of, applicable law, Affirmer hereby overtly, fully, permanently, 65 | irrevocably and unconditionally waives, abandons, and surrenders all of 66 | Affirmer's Copyright and Related Rights and associated claims and causes 67 | of action, whether now known or unknown (including existing as well as 68 | future claims and causes of action), in the Work (i) in all territories 69 | worldwide, (ii) for the maximum duration provided by applicable law or 70 | treaty (including future time extensions), (iii) in any current or future 71 | medium and for any number of copies, and (iv) for any purpose whatsoever, 72 | including without limitation commercial, advertising or promotional 73 | purposes (the "Waiver"). Affirmer makes the Waiver for the benefit of each 74 | member of the public at large and to the detriment of Affirmer's heirs and 75 | successors, fully intending that such Waiver shall not be subject to 76 | revocation, rescission, cancellation, termination, or any other legal or 77 | equitable action to disrupt the quiet enjoyment of the Work by the public 78 | as contemplated by Affirmer's express Statement of Purpose. 79 | 80 | 3. Public License Fallback. Should any part of the Waiver for any reason 81 | be judged legally invalid or ineffective under applicable law, then the 82 | Waiver shall be preserved to the maximum extent permitted taking into 83 | account Affirmer's express Statement of Purpose. In addition, to the 84 | extent the Waiver is so judged Affirmer hereby grants to each affected 85 | person a royalty-free, non transferable, non sublicensable, non exclusive, 86 | irrevocable and unconditional license to exercise Affirmer's Copyright and 87 | Related Rights in the Work (i) in all territories worldwide, (ii) for the 88 | maximum duration provided by applicable law or treaty (including future 89 | time extensions), (iii) in any current or future medium and for any number 90 | of copies, and (iv) for any purpose whatsoever, including without 91 | limitation commercial, advertising or promotional purposes (the 92 | "License"). The License shall be deemed effective as of the date CC0 was 93 | applied by Affirmer to the Work. Should any part of the License for any 94 | reason be judged legally invalid or ineffective under applicable law, such 95 | partial invalidity or ineffectiveness shall not invalidate the remainder 96 | of the License, and in such case Affirmer hereby affirms that he or she 97 | will not (i) exercise any of his or her remaining Copyright and Related 98 | Rights in the Work or (ii) assert any associated claims and causes of 99 | action with respect to the Work, in either case contrary to Affirmer's 100 | express Statement of Purpose. 101 | 102 | 4. Limitations and Disclaimers. 103 | 104 | a. No trademark or patent rights held by Affirmer are waived, abandoned, 105 | surrendered, licensed or otherwise affected by this document. 106 | b. Affirmer offers the Work as-is and makes no representations or 107 | warranties of any kind concerning the Work, express, implied, 108 | statutory or otherwise, including without limitation warranties of 109 | title, merchantability, fitness for a particular purpose, non 110 | infringement, or the absence of latent or other defects, accuracy, or 111 | the present or absence of errors, whether or not discoverable, all to 112 | the greatest extent permissible under applicable law. 113 | c. Affirmer disclaims responsibility for clearing rights of other persons 114 | that may apply to the Work or any use thereof, including without 115 | limitation any person's Copyright and Related Rights in the Work. 116 | Further, Affirmer disclaims responsibility for obtaining any necessary 117 | consents, permissions or other rights required for any use of the 118 | Work. 119 | d. Affirmer understands and acknowledges that Creative Commons is not a 120 | party to this document and has no duty or obligation with respect to 121 | this CC0 or use of the Work. 122 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROS2-Jazzy-Tutorials 2 | A repository containing tutorials for ROS2 Jazzy release with a focus on Robotics projects and labs. 3 | 4 | ## Week 1 - Pose Estimation 5 | This week covers the pose estimation lectures and also incooperates 5 lab assignments. 6 | The given Jupyter notebook covers the basic tools of `Numpy` and `Matplotlib` which are used to perform the transformations and visualize them respectively. 7 | 8 | The `task.ipynb` jupyter notebook contains the lab assignments. Implement the solutions to complete the give lab tasks 9 | 10 | -------------------------------------------------------------------------------- /Week1/.ipynb_checkpoints/task-checkpoint.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Week 1 Lab Assignments\n", 8 | "\n", 9 | "You are required to implement the following functions:\n", 10 | "- `homogenous_transform_2D` - Perform homogenous transformations for a 2D system\n", 11 | "- `homogenous_transform_3D` - Perform homogenous transformations for a 3D system\n", 12 | "- `chain_transforms` - Perform a chain of 3D transformations in the given order\n" 13 | ] 14 | }, 15 | { 16 | "cell_type": "code", 17 | "execution_count": null, 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "# import required libraries\n", 22 | "import numpy as np\n", 23 | "import matplotlib.pyplot as plt\n", 24 | "# To render plots inline\n", 25 | "%matplotlib inline\n", 26 | "\n", 27 | "# For unit tests\n", 28 | "from tester import LabTester\n", 29 | "lab_tester = LabTester()" 30 | ] 31 | }, 32 | { 33 | "cell_type": "code", 34 | "execution_count": null, 35 | "metadata": {}, 36 | "outputs": [], 37 | "source": [ 38 | "# Homogenous 2D Transformation\n", 39 | "def homogenous_transform_2D(V:np.ndarray, U:np.ndarray, theta:float) -> np.ndarray:\n", 40 | " \"\"\"\n", 41 | " Input:\n", 42 | " V - initial vector\n", 43 | " U - translation vector\n", 44 | " theta - angle in degrees\n", 45 | " Output: resulting coordinates after rotating V by theta and translating by U\n", 46 | " \"\"\"\n", 47 | "\n", 48 | " #your code here\n", 49 | " pass" 50 | ] 51 | }, 52 | { 53 | "cell_type": "markdown", 54 | "metadata": {}, 55 | "source": [ 56 | "## Lab 1: Homogeneous Transformation in 2D\n", 57 | "- *Theory*: Translating and rotating a point using homogeneous transformations.\n", 58 | "- *Task*: Implement a function that applies a 2D homogeneous transformation matrix to a given point.\n", 59 | "- *Expected Output*: Given a vector $(1,0)$ applying a translation of $(3,4)$ and a $\\theta = 45°$ rotation, the transformed point should be $(3.707, 4.707)$" 60 | ] 61 | }, 62 | { 63 | "cell_type": "code", 64 | "execution_count": null, 65 | "metadata": {}, 66 | "outputs": [], 67 | "source": [ 68 | "lab_tester.test_homogeneous_transform_2d(homogenous_transform_2D)" 69 | ] 70 | }, 71 | { 72 | "cell_type": "code", 73 | "execution_count": null, 74 | "metadata": {}, 75 | "outputs": [], 76 | "source": [ 77 | "# Homogenous 3D Transformation\n", 78 | "def homogenous_transform_3D(V:np.ndarray, U:np.ndarray, theta:dict) -> np.ndarray:\n", 79 | " \"\"\"\n", 80 | " Input:\n", 81 | " V - initial vector\n", 82 | " U - translation vector\n", 83 | " theta - dictionary of angles with the 'x', 'y' and 'z' angles\n", 84 | " Output: resulting coordinates after rotating V by theta and translating by U\n", 85 | " \"\"\"\n", 86 | "\n", 87 | " #your code here\n", 88 | " pass" 89 | ] 90 | }, 91 | { 92 | "cell_type": "markdown", 93 | "metadata": {}, 94 | "source": [ 95 | "## Lab 4: Homogeneous Transformation in 3D\n", 96 | "- *Theory*: Extending 2D transformations to 3D space.\n", 97 | "- *Task*: Implement a function that applies a 3D transformation matrix to a point in 3D.\n", 98 | "- *Expected Output*: Given a vector $(2,3,4)$ appying a translation of $(5,5,5)$ and a rotation of $\\theta = 90°$ about the z-axis the transformed point should be $(2,7,9)$." 99 | ] 100 | }, 101 | { 102 | "cell_type": "code", 103 | "execution_count": null, 104 | "metadata": {}, 105 | "outputs": [], 106 | "source": [ 107 | "lab_tester.test_homogeneous_transform_3d(homogenous_transform_3D)" 108 | ] 109 | }, 110 | { 111 | "cell_type": "markdown", 112 | "metadata": {}, 113 | "source": [ 114 | "## Lab 5: Chain Transformations\n", 115 | "- *Theory*: Combining multiple transformations using matrix multiplication.\n", 116 | "- *Task*: Implement a function that applies multiple homogeneous transformations sequentially.\n", 117 | "- *Expected Output*: Given a sequence of rotations and translations, compute the final transformed position." 118 | ] 119 | }, 120 | { 121 | "cell_type": "code", 122 | "execution_count": null, 123 | "metadata": {}, 124 | "outputs": [], 125 | "source": [ 126 | "# Chain transformations\n", 127 | "def chain_transforms(V:np.ndarray, transforms:list) -> np.ndarray:\n", 128 | " \"\"\"\n", 129 | " Input:\n", 130 | " V - initial vector\n", 131 | " transforms - list of 'translation' and 'rotation' - ex:{'translation': np.array([0, 0, 0]), 'rotation': {'z': 0}}\n", 132 | " Output: resulting coordinates after appyling the transforms in the given order\n", 133 | " \"\"\"\n", 134 | "\n", 135 | " #your code here\n", 136 | " pass" 137 | ] 138 | }, 139 | { 140 | "cell_type": "code", 141 | "execution_count": null, 142 | "metadata": {}, 143 | "outputs": [], 144 | "source": [ 145 | "lab_tester.test_chain_transformations(chain_transforms)" 146 | ] 147 | }, 148 | { 149 | "cell_type": "code", 150 | "execution_count": null, 151 | "metadata": {}, 152 | "outputs": [], 153 | "source": [ 154 | "# Summary of test results\n", 155 | "lab_tester.print_summary()" 156 | ] 157 | }, 158 | { 159 | "cell_type": "markdown", 160 | "metadata": {}, 161 | "source": [] 162 | } 163 | ], 164 | "metadata": { 165 | "kernelspec": { 166 | "display_name": "base", 167 | "language": "python", 168 | "name": "python3" 169 | }, 170 | "language_info": { 171 | "codemirror_mode": { 172 | "name": "ipython", 173 | "version": 3 174 | }, 175 | "file_extension": ".py", 176 | "mimetype": "text/x-python", 177 | "name": "python", 178 | "nbconvert_exporter": "python", 179 | "pygments_lexer": "ipython3", 180 | "version": "3.12.7" 181 | } 182 | }, 183 | "nbformat": 4, 184 | "nbformat_minor": 2 185 | } 186 | -------------------------------------------------------------------------------- /Week1/.ipynb_checkpoints/tester-checkpoint.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from typing import Callable, List, Union 3 | import inspect 4 | 5 | 6 | class LabTester: 7 | """ 8 | A test helper class for validating 2D/3D rotation and transformation implementations 9 | """ 10 | 11 | def __init__(self): 12 | self.passed_tests = 0 13 | self.total_tests = 0 14 | 15 | def assert_array_almost_equal( 16 | self, 17 | actual: np.ndarray, 18 | expected: np.ndarray, 19 | decimal: int = 6, 20 | message: str = "", 21 | ): 22 | """Custom assertion for numpy arrays with detailed output""" 23 | self.total_tests += 1 24 | try: 25 | np.testing.assert_array_almost_equal(actual, expected, decimal=decimal) 26 | print(f"✅ {message}") 27 | self.passed_tests += 1 28 | except AssertionError as e: 29 | print(f"❌ {message}") 30 | print(f" Expected: {expected}") 31 | print(f" Got: {actual}") 32 | 33 | def test_homogeneous_transform_2d(self, homogenous_transform_2D: Callable): 34 | """Test suite for 2D homogeneous transformation implementation""" 35 | print("\n=== Testing 2D Homogeneous Transformation Implementation ===") 36 | 37 | # Test 1: Given example - translation(3,4) + rotation(45°) 38 | point = np.array([1, 0]) 39 | translation = np.array([3, 4]) 40 | transformed = homogenous_transform_2D(point, translation, 45) 41 | self.assert_array_almost_equal( 42 | transformed, 43 | [3.707, 4.707], 44 | decimal=3, 45 | message="Combined transform: translation(3,4) + rotation(45°)", 46 | ) 47 | 48 | # Test 2: Pure translation 49 | point = np.array([1, 2]) 50 | translation = np.array([3, 4]) 51 | transformed = homogenous_transform_2D(point, translation, 0) 52 | self.assert_array_almost_equal( 53 | transformed, [4, 6], message="Pure translation: no rotation" 54 | ) 55 | 56 | # Test 3: Pure rotation 57 | point = np.array([1, 1]) 58 | translation = np.array([0, 0]) 59 | transformed = homogenous_transform_2D(point, translation, 30) 60 | self.assert_array_almost_equal( 61 | transformed, 62 | [0.366, 1.366], 63 | decimal=3, 64 | message="Pure rotation: no translation", 65 | ) 66 | 67 | # Test 4: Rotate and translate 68 | point = np.array([1, 1]) 69 | translation = np.array([4, 3]) 70 | transformed = homogenous_transform_2D(point, translation, 90) 71 | self.assert_array_almost_equal( 72 | transformed, [3, 4], decimal=0, message="Translation and rotation" 73 | ) 74 | 75 | # Test 3: Pure rotation 76 | point = np.array([1, 0]) 77 | translation = np.array([0, 0]) 78 | transformed = homogenous_transform_2D(point, translation, 90) 79 | self.assert_array_almost_equal( 80 | transformed, [0, 1], message="Pure rotation: no translation" 81 | ) 82 | 83 | # Test 4: Combined rotation and translation 84 | point = np.array([1, 0]) 85 | translation = np.array([2.25, 3.75]) 86 | transformed = homogenous_transform_2D(point, translation, 90) 87 | self.assert_array_almost_equal( 88 | transformed, 89 | [2.250, 4.750], 90 | decimal=3, 91 | message="Combined rotation and translation", 92 | ) 93 | 94 | # Test 5: Combined rotation and translation 95 | point = np.array([4, 3]) 96 | translation = np.array([2, 3]) 97 | transformed = homogenous_transform_2D(point, translation, 45) 98 | self.assert_array_almost_equal( 99 | transformed, 100 | [2.7071, 7.9498], 101 | decimal=3, 102 | message="Combined rotation and translation", 103 | ) 104 | 105 | # Test 6: Combined rotation and translation far from origin 106 | point = np.array([10, 15]) 107 | translation = np.array([100, 150]) 108 | transformed = homogenous_transform_2D(point, translation, 60) 109 | self.assert_array_almost_equal( 110 | transformed, 111 | [92.00962, 166.16025], 112 | decimal=3, 113 | message="Combined rotation and translation", 114 | ) 115 | 116 | def test_homogeneous_transform_3d(self, homogenous_transform_3D: Callable): 117 | """ 118 | Test suite for 3D homogeneous transformation implementation 119 | 120 | Parameters: 121 | homogenous_transform_3D: function that takes (point: np.ndarray, translation: np.ndarray, 122 | rotation_angles: dict) and returns transformed point 123 | """ 124 | print("\n=== Testing 3D Homogeneous Transformation Implementation ===") 125 | 126 | # Test 1: Given example - translation(5,5,5) + rotation(90° about z-axis) 127 | point = np.array([2, 3, 4]) 128 | translation = np.array([5, 5, 5]) 129 | rotation = {"z": 90} # rotation about z-axis 130 | transformed = homogenous_transform_3D(point, translation, rotation) 131 | self.assert_array_almost_equal( 132 | transformed, 133 | [2, 7, 9], 134 | decimal=3, 135 | message="Combined 3D transform: translation(5,5,5) + rotation(90° about z)", 136 | ) 137 | 138 | # Test 2: Pure translation in 3D 139 | point = np.array([1, 2, 3]) 140 | translation = np.array([4, 5, 6]) 141 | rotation = {"x": 0, "y": 0, "z": 0} 142 | transformed = homogenous_transform_3D(point, translation, rotation) 143 | self.assert_array_almost_equal( 144 | transformed, [5, 7, 9], message="Pure 3D translation: no rotation" 145 | ) 146 | 147 | # Test 3: Pure rotation about x-axis 148 | point = np.array([1, 1, 0]) 149 | translation = np.array([0, 0, 0]) 150 | rotation = {"x": 90} 151 | transformed = homogenous_transform_3D(point, translation, rotation) 152 | self.assert_array_almost_equal( 153 | transformed, [1, 0, 1], decimal=3, message="Pure rotation about x-axis" 154 | ) 155 | 156 | # Test 4: Combined rotations 157 | point = np.array([1, 0, 0]) 158 | translation = np.array([0, 0, 0]) 159 | rotation = {"x": 90, "y": 90, "z": 90} 160 | transformed = homogenous_transform_3D(point, translation, rotation) 161 | self.assert_array_almost_equal( 162 | transformed, 163 | [0, 0, 1], 164 | decimal=3, 165 | message="Combined rotations about x, y, and z axes", 166 | ) 167 | 168 | # Test 5: Combined rotations 169 | point = np.array([1, 2, 3]) 170 | translation = np.array([-1, -1, -1]) 171 | rotation = {"x": 90, "y": 90, "z": 90} 172 | transformed = homogenous_transform_3D(point, translation, rotation) 173 | self.assert_array_almost_equal( 174 | transformed, 175 | [2, -3, 0], 176 | decimal=3, 177 | message="Rotations with Negative Translation", 178 | ) 179 | 180 | # Test 6: Combined rotations 181 | point = np.array([1, 2, 3]) 182 | translation = np.array([-5, 5, -1]) 183 | rotation = {"x": 60, "y": 90, "z": 30} 184 | transformed = homogenous_transform_3D(point, translation, rotation) 185 | self.assert_array_almost_equal( 186 | transformed, 187 | [-2, 6, 1], 188 | decimal=3, 189 | message="Rotations by odd angles with Translation", 190 | ) 191 | 192 | # Test 7: Combined rotations 193 | point = np.array([1, 2, 3]) 194 | translation = np.array([0, 0, 0]) 195 | rotation = {"x": 45, "y": 30, "z": 60} 196 | transformed = homogenous_transform_3D(point, translation, rotation) 197 | self.assert_array_almost_equal( 198 | transformed, 199 | [0.43301, -0.95323, 3.59219], 200 | decimal=3, 201 | message="Combined rotations about x, y, and z axes", 202 | ) 203 | 204 | # Test 8: Combined rotations 205 | point = np.array([1, 2, 3]) 206 | translation = np.array([1, 1, 1]) 207 | rotation = {"x": 45, "y": 60, "z": 30} 208 | transformed = homogenous_transform_3D(point, translation, rotation) 209 | self.assert_array_almost_equal( 210 | transformed, 211 | [3.53109, 1.4356, 3.721], 212 | decimal=3, 213 | message="Combined rotations about x, y, and z axes with translation", 214 | ) 215 | 216 | # Test 9: Combined rotations far from origin 217 | point = np.array([10, 15, 20]) 218 | translation = np.array([100, 150, 200]) 219 | rotation = {"x": 45, "y": 60, "z": 30} 220 | transformed = homogenous_transform_3D(point, translation, rotation) 221 | self.assert_array_almost_equal( 222 | transformed, 223 | [117.90064, 156.36056, 219.08168], 224 | decimal=3, 225 | message="Combined rotations about x, y, and z axes with translation", 226 | ) 227 | 228 | # Test 10: Pure rotations of big angles 229 | point = np.array([1, 0, 1]) 230 | translation = np.array([0, 0, 0]) 231 | rotation = {"x": 120, "y": 150, "z": 250} 232 | transformed = homogenous_transform_3D(point, translation, rotation) 233 | self.assert_array_almost_equal( 234 | transformed, 235 | [0.7962, 1.07175, -0.46629], 236 | decimal=3, 237 | message="Pure rotations of big angles", 238 | ) 239 | 240 | # Test 11: Combined rotations about negative angles with translation 241 | point = np.array([1, 0, 0]) 242 | translation = np.array([1, 1, 1]) 243 | rotation = {"x": -45, "y": -60, "z": -30} 244 | transformed = homogenous_transform_3D(point, translation, rotation) 245 | self.assert_array_almost_equal( 246 | transformed, 247 | [1.43301, 1.17678, 1.88388], 248 | decimal=3, 249 | message="Combined rotations about negative x, y, and z axes with translation", 250 | ) 251 | 252 | def test_chain_transformations(self, chain_transforms: Callable): 253 | """ 254 | Test suite for chained transformation implementation 255 | 256 | Parameters: 257 | chain_transforms: function that takes (point: np.ndarray, transformations: List[dict]) 258 | where each dict contains 'translation' and 'rotation' keys 259 | """ 260 | print("\n=== Testing Chain Transformations Implementation ===") 261 | 262 | # Test 1: Simple chain - two translations 263 | point = np.array([1, 1, 1]) 264 | transforms = [ 265 | {"translation": np.array([1, 0, 0]), "rotation": {"z": 0}}, 266 | {"translation": np.array([0, 1, 0]), "rotation": {"z": 0}}, 267 | ] 268 | transformed = chain_transforms(point, transforms) 269 | self.assert_array_almost_equal( 270 | transformed, [2, 2, 1], message="Chain of two translations" 271 | ) 272 | 273 | # Test 2: Translation followed by rotation 274 | point = np.array([1, 0, 0]) 275 | transforms = [ 276 | {"translation": np.array([1, 0, 0]), "rotation": {"z": 0}}, 277 | {"translation": np.array([0, 0, 0]), "rotation": {"z": 90}}, 278 | ] 279 | transformed = chain_transforms(point, transforms) 280 | self.assert_array_almost_equal( 281 | transformed, 282 | [0, 2, 0], 283 | decimal=3, 284 | message="Translation followed by rotation", 285 | ) 286 | 287 | # Test 3: Complex chain 288 | point = np.array([1, 0, 0]) 289 | transforms = [ 290 | {"translation": np.array([1, 1, 1]), "rotation": {"x": 90}}, 291 | {"translation": np.array([0, 0, 1]), "rotation": {"y": 90}}, 292 | {"translation": np.array([1, 0, 0]), "rotation": {"z": 90}}, 293 | ] 294 | transformed = chain_transforms(point, transforms) 295 | self.assert_array_almost_equal( 296 | transformed, 297 | [0, 1, -1], 298 | decimal=3, 299 | message="Complex chain of transformations", 300 | ) 301 | 302 | # Test 4: Identity chain 303 | point = np.array([1, 2, 3]) 304 | transforms = [ 305 | {"translation": np.array([0, 0, 0]), "rotation": {"x": 0}}, 306 | {"translation": np.array([0, 0, 0]), "rotation": {"y": 0}}, 307 | {"translation": np.array([0, 0, 0]), "rotation": {"z": 0}}, 308 | ] 309 | transformed = chain_transforms(point, transforms) 310 | self.assert_array_almost_equal( 311 | transformed, point, message="Chain of identity transformations" 312 | ) 313 | 314 | # Test 5: Complex chains 315 | point = np.array([1, 2, 3]) 316 | transforms = [ 317 | { 318 | "translation": np.array([1, 0, -1]), 319 | "rotation": {"x": 45, "y": 30, "z": 60}, 320 | }, 321 | { 322 | "translation": np.array([-1, 1, 0]), 323 | "rotation": {"x": 135, "y": 120, "z": 30}, 324 | }, 325 | { 326 | "translation": np.array([0, -1, 1]), 327 | "rotation": {"x": -90, "y": 60, "z": 0}, 328 | }, 329 | ] 330 | transformed = chain_transforms(point, transforms) 331 | self.assert_array_almost_equal( 332 | transformed, 333 | [1.8308927, -0.38874408, -2.0454028], 334 | decimal=3, 335 | message="Complex transformations", 336 | ) 337 | 338 | # Test 6: 339 | point = np.array([1, 1, 1]) 340 | transforms = [ 341 | { 342 | "translation": np.array([1, 1, 1]), 343 | "rotation": {"x": 45, "y": 45, "z": 45}, 344 | }, 345 | { 346 | "translation": np.array([-1, -1, -1]), 347 | "rotation": {"x": -45, "y": -45, "z": -45}, 348 | }, 349 | ] 350 | transformed = chain_transforms(point, transforms) 351 | self.assert_array_almost_equal( 352 | transformed, 353 | [-1.16421356, 1.28033009, 1.48743687], 354 | decimal=4, 355 | message="Tranform * Reverse transform", 356 | ) 357 | 358 | # Test 6: Complex chains 359 | point = np.array([1, 2, 3]) 360 | transforms = [ 361 | { 362 | "translation": np.array([1, 0, -1]), 363 | "rotation": {"x": 45, "y": 30, "z": 60}, 364 | }, 365 | { 366 | "translation": np.array([-1, 1, 0]), 367 | "rotation": {"x": 135, "y": 120, "z": 30}, 368 | }, 369 | { 370 | "translation": np.array([0, -1, 1]), 371 | "rotation": {"x": -90, "y": 60, "z": 0}, 372 | }, 373 | { 374 | "translation": np.array([1, 1, 1]), 375 | "rotation": {"x": 90, "y": 90, "z": 90}, 376 | }, 377 | { 378 | "translation": np.array([1, 1, 0]), 379 | "rotation": {"x": 135, "y": 120, "z": 30}, 380 | }, 381 | { 382 | "translation": np.array([0, 1, -1]), 383 | "rotation": {"x": -90, "y": 60, "z": 0}, 384 | }, 385 | ] 386 | transformed = chain_transforms(point, transforms) 387 | self.assert_array_almost_equal( 388 | transformed, 389 | [2.56055, -2.43085, -1.54043], 390 | decimal=3, 391 | message="Complex transformations with additional transform", 392 | ) 393 | 394 | # Test 7: Complex chains 395 | point = np.array([4, 5, 6]) 396 | transforms = [ 397 | { 398 | "translation": np.array([1, 0, -1]), 399 | "rotation": {"x": 150, "y": 130, "z": 220}, 400 | }, 401 | { 402 | "translation": np.array([-1, 1, 0]), 403 | "rotation": {"x": 135, "y": 120, "z": 90}, 404 | }, 405 | { 406 | "translation": np.array([0, -1, 1]), 407 | "rotation": {"x": 120, "y": 270, "z": 110}, 408 | }, 409 | ] 410 | transformed = chain_transforms(point, transforms) 411 | self.assert_array_almost_equal( 412 | transformed, 413 | [0.99092, -9.01667, 1.72435], 414 | decimal=3, 415 | message="Complex transformations with big angles", 416 | ) 417 | 418 | # Test 8: Complex chains 419 | point = np.array([3, 6, 9]) 420 | transforms = [ 421 | { 422 | "translation": np.array([1, 0, -1]), 423 | "rotation": {"x": -45, "y": 30, "z": -60}, 424 | }, 425 | { 426 | "translation": np.array([-1, 1, 0]), 427 | "rotation": {"x": -60, "y": -30, "z": -90}, 428 | }, 429 | { 430 | "translation": np.array([0, -1, 1]), 431 | "rotation": {"x": -45, "y": 30, "z": -30}, 432 | }, 433 | { 434 | "translation": np.array([1, 1, 1]), 435 | "rotation": {"x": 45, "y": -60, "z": -45}, 436 | }, 437 | { 438 | "translation": np.array([1, 1, 0]), 439 | "rotation": {"x": -30, "y": 60, "z": 30}, 440 | }, 441 | { 442 | "translation": np.array([0, 1, -1]), 443 | "rotation": {"x": -45, "y": 30, "z": -30}, 444 | }, 445 | ] 446 | transformed = chain_transforms(point, transforms) 447 | self.assert_array_almost_equal( 448 | transformed, 449 | [2.56055, -2.43085, -1.54043], 450 | decimal=3, 451 | message="Complex transformations with nagative angles and additional transform", 452 | ) 453 | 454 | def print_summary(self): 455 | """Print summary of test results""" 456 | print(f"\n=== Test Summary ===") 457 | print(f"Passed: {self.passed_tests}/{self.total_tests} tests") 458 | if self.passed_tests == self.total_tests: 459 | print("🎉 All tests passed!") 460 | else: 461 | print(f"❌ {self.total_tests - self.passed_tests} tests failed") 462 | -------------------------------------------------------------------------------- /Week1/__pycache__/tester.cpython-312.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRO-GUNE/ROS2-Jazzy-Tutorials/c7b987c7cf375093db2b0c1744ac84cb52c528fb/Week1/__pycache__/tester.cpython-312.pyc -------------------------------------------------------------------------------- /Week1/task.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Week 1 Lab Assignments\n", 8 | "\n", 9 | "You are required to implement the following functions:\n", 10 | "- `homogenous_transform_2D` - Perform homogenous transformations for a 2D system\n", 11 | "- `homogenous_transform_3D` - Perform homogenous transformations for a 3D system\n", 12 | "- `chain_transforms` - Perform a chain of 3D transformations in the given order\n" 13 | ] 14 | }, 15 | { 16 | "cell_type": "code", 17 | "execution_count": null, 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "# import required libraries\n", 22 | "import numpy as np\n", 23 | "import matplotlib.pyplot as plt\n", 24 | "# To render plots inline\n", 25 | "%matplotlib inline\n", 26 | "\n", 27 | "# For unit tests\n", 28 | "from tester import LabTester\n", 29 | "lab_tester = LabTester()" 30 | ] 31 | }, 32 | { 33 | "cell_type": "code", 34 | "execution_count": null, 35 | "metadata": {}, 36 | "outputs": [], 37 | "source": [ 38 | "# Homogenous 2D Transformation\n", 39 | "def homogenous_transform_2D(V:np.ndarray, U:np.ndarray, theta:float) -> np.ndarray:\n", 40 | " \"\"\"\n", 41 | " Input:\n", 42 | " V - initial vector\n", 43 | " U - translation vector\n", 44 | " theta - angle in degrees\n", 45 | " Output: resulting coordinates after rotating V by theta and translating by U\n", 46 | " \"\"\"\n", 47 | "\n", 48 | " #your code here\n", 49 | " pass" 50 | ] 51 | }, 52 | { 53 | "cell_type": "markdown", 54 | "metadata": {}, 55 | "source": [ 56 | "## Lab 1: Homogeneous Transformation in 2D\n", 57 | "- *Theory*: Translating and rotating a point using homogeneous transformations.\n", 58 | "- *Task*: Implement a function that applies a 2D homogeneous transformation matrix to a given point.\n", 59 | "- *Expected Output*: Given a vector $(1,0)$ applying a translation of $(3,4)$ and a $\\theta = 45°$ rotation, the transformed point should be $(3.707, 4.707)$" 60 | ] 61 | }, 62 | { 63 | "cell_type": "code", 64 | "execution_count": null, 65 | "metadata": {}, 66 | "outputs": [], 67 | "source": [ 68 | "lab_tester.test_homogeneous_transform_2d(homogenous_transform_2D)" 69 | ] 70 | }, 71 | { 72 | "cell_type": "code", 73 | "execution_count": null, 74 | "metadata": {}, 75 | "outputs": [], 76 | "source": [ 77 | "# Homogenous 3D Transformation\n", 78 | "def homogenous_transform_3D(V:np.ndarray, U:np.ndarray, theta:dict) -> np.ndarray:\n", 79 | " \"\"\"\n", 80 | " Input:\n", 81 | " V - initial vector\n", 82 | " U - translation vector\n", 83 | " theta - dictionary of angles with the 'x', 'y' and 'z' angles\n", 84 | " Output: resulting coordinates after rotating V by theta and translating by U\n", 85 | " \"\"\"\n", 86 | "\n", 87 | " #your code here\n", 88 | " pass" 89 | ] 90 | }, 91 | { 92 | "cell_type": "markdown", 93 | "metadata": {}, 94 | "source": [ 95 | "## Lab 4: Homogeneous Transformation in 3D\n", 96 | "- *Theory*: Extending 2D transformations to 3D space.\n", 97 | "- *Task*: Implement a function that applies a 3D transformation matrix to a point in 3D.\n", 98 | "- *Expected Output*: Given a vector $(2,3,4)$ appying a translation of $(5,5,5)$ and a rotation of $\\theta = 90°$ about the z-axis the transformed point should be $(2,7,9)$." 99 | ] 100 | }, 101 | { 102 | "cell_type": "code", 103 | "execution_count": null, 104 | "metadata": {}, 105 | "outputs": [], 106 | "source": [ 107 | "lab_tester.test_homogeneous_transform_3d(homogenous_transform_3D)" 108 | ] 109 | }, 110 | { 111 | "cell_type": "markdown", 112 | "metadata": {}, 113 | "source": [ 114 | "## Lab 5: Chain Transformations\n", 115 | "- *Theory*: Combining multiple transformations using matrix multiplication.\n", 116 | "- *Task*: Implement a function that applies multiple homogeneous transformations sequentially.\n", 117 | "- *Expected Output*: Given a sequence of rotations and translations, compute the final transformed position." 118 | ] 119 | }, 120 | { 121 | "cell_type": "code", 122 | "execution_count": null, 123 | "metadata": {}, 124 | "outputs": [], 125 | "source": [ 126 | "# Chain transformations\n", 127 | "def chain_transforms(V:np.ndarray, transforms:list) -> np.ndarray:\n", 128 | " \"\"\"\n", 129 | " Input:\n", 130 | " V - initial vector\n", 131 | " transforms - list of 'translation' and 'rotation' - ex:{'translation': np.array([0, 0, 0]), 'rotation': {'z': 0}}\n", 132 | " Output: resulting coordinates after appyling the transforms in the given order\n", 133 | " \"\"\"\n", 134 | "\n", 135 | " #your code here\n", 136 | " pass" 137 | ] 138 | }, 139 | { 140 | "cell_type": "code", 141 | "execution_count": null, 142 | "metadata": {}, 143 | "outputs": [], 144 | "source": [ 145 | "lab_tester.test_chain_transformations(chain_transforms)" 146 | ] 147 | }, 148 | { 149 | "cell_type": "code", 150 | "execution_count": null, 151 | "metadata": {}, 152 | "outputs": [], 153 | "source": [ 154 | "# Summary of test results\n", 155 | "lab_tester.print_summary()" 156 | ] 157 | }, 158 | { 159 | "cell_type": "markdown", 160 | "metadata": {}, 161 | "source": [] 162 | } 163 | ], 164 | "metadata": { 165 | "kernelspec": { 166 | "display_name": "base", 167 | "language": "python", 168 | "name": "python3" 169 | }, 170 | "language_info": { 171 | "codemirror_mode": { 172 | "name": "ipython", 173 | "version": 3 174 | }, 175 | "file_extension": ".py", 176 | "mimetype": "text/x-python", 177 | "name": "python", 178 | "nbconvert_exporter": "python", 179 | "pygments_lexer": "ipython3", 180 | "version": "3.12.7" 181 | } 182 | }, 183 | "nbformat": 4, 184 | "nbformat_minor": 2 185 | } 186 | -------------------------------------------------------------------------------- /Week1/tester.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from typing import Callable, List, Union 3 | import inspect 4 | 5 | 6 | class LabTester: 7 | """ 8 | A test helper class for validating 2D/3D rotation and transformation implementations 9 | """ 10 | 11 | def __init__(self): 12 | self.passed_tests = 0 13 | self.total_tests = 0 14 | 15 | def assert_array_almost_equal( 16 | self, 17 | actual: np.ndarray, 18 | expected: np.ndarray, 19 | decimal: int = 6, 20 | message: str = "", 21 | ): 22 | """Custom assertion for numpy arrays with detailed output""" 23 | self.total_tests += 1 24 | try: 25 | np.testing.assert_array_almost_equal(actual, expected, decimal=decimal) 26 | print(f"✅ {message}") 27 | self.passed_tests += 1 28 | except AssertionError as e: 29 | print(f"❌ {message}") 30 | print(f" Expected: {expected}") 31 | print(f" Got: {actual}") 32 | 33 | def test_homogeneous_transform_2d(self, homogenous_transform_2D: Callable): 34 | """Test suite for 2D homogeneous transformation implementation""" 35 | print("\n=== Testing 2D Homogeneous Transformation Implementation ===") 36 | 37 | # Test 1: Given example - translation(3,4) + rotation(45°) 38 | point = np.array([1, 0]) 39 | translation = np.array([3, 4]) 40 | transformed = homogenous_transform_2D(point, translation, 45) 41 | self.assert_array_almost_equal( 42 | transformed, 43 | [3.707, 4.707], 44 | decimal=3, 45 | message="Combined transform: translation(3,4) + rotation(45°)", 46 | ) 47 | 48 | # Test 2: Pure translation 49 | point = np.array([1, 2]) 50 | translation = np.array([3, 4]) 51 | transformed = homogenous_transform_2D(point, translation, 0) 52 | self.assert_array_almost_equal( 53 | transformed, [4, 6], message="Pure translation: no rotation" 54 | ) 55 | 56 | # Test 3: Pure rotation 57 | point = np.array([1, 1]) 58 | translation = np.array([0, 0]) 59 | transformed = homogenous_transform_2D(point, translation, 30) 60 | self.assert_array_almost_equal( 61 | transformed, 62 | [0.366, 1.366], 63 | decimal=3, 64 | message="Pure rotation: no translation", 65 | ) 66 | 67 | # Test 4: Rotate and translate 68 | point = np.array([1, 1]) 69 | translation = np.array([4, 3]) 70 | transformed = homogenous_transform_2D(point, translation, 90) 71 | self.assert_array_almost_equal( 72 | transformed, [3, 4], decimal=0, message="Translation and rotation" 73 | ) 74 | 75 | # Test 3: Pure rotation 76 | point = np.array([1, 0]) 77 | translation = np.array([0, 0]) 78 | transformed = homogenous_transform_2D(point, translation, 90) 79 | self.assert_array_almost_equal( 80 | transformed, [0, 1], message="Pure rotation: no translation" 81 | ) 82 | 83 | # Test 4: Combined rotation and translation 84 | point = np.array([1, 0]) 85 | translation = np.array([2.25, 3.75]) 86 | transformed = homogenous_transform_2D(point, translation, 90) 87 | self.assert_array_almost_equal( 88 | transformed, 89 | [2.250, 4.750], 90 | decimal=3, 91 | message="Combined rotation and translation", 92 | ) 93 | 94 | # Test 5: Combined rotation and translation 95 | point = np.array([4, 3]) 96 | translation = np.array([2, 3]) 97 | transformed = homogenous_transform_2D(point, translation, 45) 98 | self.assert_array_almost_equal( 99 | transformed, 100 | [2.7071, 7.9498], 101 | decimal=3, 102 | message="Combined rotation and translation", 103 | ) 104 | 105 | # Test 6: Combined rotation and translation far from origin 106 | point = np.array([10, 15]) 107 | translation = np.array([100, 150]) 108 | transformed = homogenous_transform_2D(point, translation, 60) 109 | self.assert_array_almost_equal( 110 | transformed, 111 | [92.00962, 166.16025], 112 | decimal=3, 113 | message="Combined rotation and translation", 114 | ) 115 | 116 | # Test 7: Large rotations 117 | point = np.array([4, 3]) 118 | translation = np.array([1, 1]) 119 | transformed = homogenous_transform_2D(point, translation, 190) 120 | self.assert_array_almost_equal( 121 | transformed, [-2.41828648, -2.64901597],decimal=3, 122 | message="Large rotaions" 123 | ) 124 | 125 | # Test 8 126 | point = np.array([2, 4]) 127 | translation = np.array([-2, -1]) 128 | transformed = homogenous_transform_2D(point, translation, 75) 129 | self.assert_array_almost_equal( 130 | transformed, [-5.34606521, 1.96712783],decimal=3, 131 | message="Combined transformation" 132 | ) 133 | 134 | # Test 9 135 | point = np.array([-2, 2]) 136 | translation = np.array([2, -2]) 137 | transformed = homogenous_transform_2D(point, translation, 90) 138 | self.assert_array_almost_equal( 139 | transformed, [0, -4],decimal=3, 140 | message="Combined transformation" 141 | ) 142 | 143 | def test_homogeneous_transform_3d(self, homogenous_transform_3D: Callable): 144 | """ 145 | Test suite for 3D homogeneous transformation implementation 146 | 147 | Parameters: 148 | homogenous_transform_3D: function that takes (point: np.ndarray, translation: np.ndarray, 149 | rotation_angles: dict) and returns transformed point 150 | """ 151 | print("\n=== Testing 3D Homogeneous Transformation Implementation ===") 152 | 153 | # Test 1: Given example - translation(5,5,5) + rotation(90° about z-axis) 154 | point = np.array([2, 3, 4]) 155 | translation = np.array([5, 5, 5]) 156 | rotation = {"z": 90} # rotation about z-axis 157 | transformed = homogenous_transform_3D(point, translation, rotation) 158 | self.assert_array_almost_equal( 159 | transformed, 160 | [2, 7, 9], 161 | decimal=3, 162 | message="Combined 3D transform: translation(5,5,5) + rotation(90° about z)", 163 | ) 164 | 165 | # Test 2: Pure translation in 3D 166 | point = np.array([1, 2, 3]) 167 | translation = np.array([4, 5, 6]) 168 | rotation = {"x": 0, "y": 0, "z": 0} 169 | transformed = homogenous_transform_3D(point, translation, rotation) 170 | self.assert_array_almost_equal( 171 | transformed, [5, 7, 9], message="Pure 3D translation: no rotation" 172 | ) 173 | 174 | # Test 3: Pure rotation about x-axis 175 | point = np.array([1, 1, 0]) 176 | translation = np.array([0, 0, 0]) 177 | rotation = {"x": 90} 178 | transformed = homogenous_transform_3D(point, translation, rotation) 179 | self.assert_array_almost_equal( 180 | transformed, [1, 0, 1], decimal=3, message="Pure rotation about x-axis" 181 | ) 182 | 183 | # Test 4: Combined rotations 184 | point = np.array([1, 0, 0]) 185 | translation = np.array([0, 0, 0]) 186 | rotation = {"x": 90, "y": 90, "z": 90} 187 | transformed = homogenous_transform_3D(point, translation, rotation) 188 | self.assert_array_almost_equal( 189 | transformed, 190 | [0, 0, 1], 191 | decimal=3, 192 | message="Combined rotations about x, y, and z axes", 193 | ) 194 | 195 | # Test 5: Combined rotations 196 | point = np.array([1, 2, 3]) 197 | translation = np.array([-1, -1, -1]) 198 | rotation = {"x": 90, "y": 90, "z": 90} 199 | transformed = homogenous_transform_3D(point, translation, rotation) 200 | self.assert_array_almost_equal( 201 | transformed, 202 | [2, -3, 0], 203 | decimal=3, 204 | message="Rotations with Negative Translation", 205 | ) 206 | 207 | # Test 6: Combined rotations 208 | point = np.array([1, 2, 3]) 209 | translation = np.array([-5, 5, -1]) 210 | rotation = {"x": 60, "y": 90, "z": 30} 211 | transformed = homogenous_transform_3D(point, translation, rotation) 212 | self.assert_array_almost_equal( 213 | transformed, 214 | [-2, 6, 1], 215 | decimal=3, 216 | message="Rotations by odd angles with Translation", 217 | ) 218 | 219 | # Test 7: Combined rotations 220 | point = np.array([1, 2, 3]) 221 | translation = np.array([0, 0, 0]) 222 | rotation = {"x": 45, "y": 30, "z": 60} 223 | transformed = homogenous_transform_3D(point, translation, rotation) 224 | self.assert_array_almost_equal( 225 | transformed, 226 | [0.43301, -0.95323, 3.59219], 227 | decimal=3, 228 | message="Combined rotations about x, y, and z axes", 229 | ) 230 | 231 | # Test 8: Combined rotations 232 | point = np.array([1, 2, 3]) 233 | translation = np.array([1, 1, 1]) 234 | rotation = {"x": 45, "y": 60, "z": 30} 235 | transformed = homogenous_transform_3D(point, translation, rotation) 236 | self.assert_array_almost_equal( 237 | transformed, 238 | [3.53109, 1.4356, 3.721], 239 | decimal=3, 240 | message="Combined rotations about x, y, and z axes with translation", 241 | ) 242 | 243 | # Test 9: Combined rotations far from origin 244 | point = np.array([10, 15, 20]) 245 | translation = np.array([100, 150, 200]) 246 | rotation = {"x": 45, "y": 60, "z": 30} 247 | transformed = homogenous_transform_3D(point, translation, rotation) 248 | self.assert_array_almost_equal( 249 | transformed, 250 | [117.90064, 156.36056, 219.08168], 251 | decimal=3, 252 | message="Combined rotations about x, y, and z axes with translation", 253 | ) 254 | 255 | # Test 10: Pure rotations of big angles 256 | point = np.array([1, 0, 1]) 257 | translation = np.array([0, 0, 0]) 258 | rotation = {"x": 120, "y": 150, "z": 250} 259 | transformed = homogenous_transform_3D(point, translation, rotation) 260 | self.assert_array_almost_equal( 261 | transformed, 262 | [0.7962, 1.07175, -0.46629], 263 | decimal=3, 264 | message="Pure rotations of big angles", 265 | ) 266 | 267 | # Test 11: Combined rotations about negative angles with translation 268 | point = np.array([1, 0, 0]) 269 | translation = np.array([1, 1, 1]) 270 | rotation = {"x": -45, "y": -60, "z": -30} 271 | transformed = homogenous_transform_3D(point, translation, rotation) 272 | self.assert_array_almost_equal( 273 | transformed, 274 | [1.43301, 1.17678, 1.88388], 275 | decimal=3, 276 | message="Combined rotations about negative x, y, and z axes with translation", 277 | ) 278 | 279 | # Test 12: Identity transformation 280 | point = np.array([2, 2, 3]) 281 | translation = np.array([0, 0, 0]) 282 | rotation = {'x': 0, 'y': 0, 'z': 0} 283 | transformed = homogenous_transform_3D(point, translation, rotation) 284 | self.assert_array_almost_equal( 285 | transformed, [2, 2, 3], decimal=3, 286 | message="Identity transformation" 287 | ) 288 | 289 | # Test 13: Large rotations 290 | point = np.array([2, 1, 3]) 291 | translation = np.array([1, 0, 2]) 292 | rotation = {'x':360, 'y': 120, 'z': 240} 293 | transformed = homogenous_transform_3D(point, translation, rotation) 294 | self.assert_array_almost_equal( 295 | transformed, [3.66506351, -2.23205081, 0.6160254], decimal=3, 296 | message="Large rotations" 297 | ) 298 | 299 | def test_chain_transformations(self, chain_transforms: Callable): 300 | """ 301 | Test suite for chained transformation implementation 302 | 303 | Parameters: 304 | chain_transforms: function that takes (point: np.ndarray, transformations: List[dict]) 305 | where each dict contains 'translation' and 'rotation' keys 306 | """ 307 | print("\n=== Testing Chain Transformations Implementation ===") 308 | 309 | # Test 1: Simple chain - two translations 310 | point = np.array([1, 1, 1]) 311 | transforms = [ 312 | {"translation": np.array([1, 0, 0]), "rotation": {"z": 0}}, 313 | {"translation": np.array([0, 1, 0]), "rotation": {"z": 0}}, 314 | ] 315 | transformed = chain_transforms(point, transforms) 316 | self.assert_array_almost_equal( 317 | transformed, [2, 2, 1], message="Chain of two translations" 318 | ) 319 | 320 | # Test 2: Translation followed by rotation 321 | point = np.array([1, 0, 0]) 322 | transforms = [ 323 | {"translation": np.array([1, 0, 0]), "rotation": {"z": 0}}, 324 | {"translation": np.array([0, 0, 0]), "rotation": {"z": 90}}, 325 | ] 326 | transformed = chain_transforms(point, transforms) 327 | self.assert_array_almost_equal( 328 | transformed, 329 | [0, 2, 0], 330 | decimal=3, 331 | message="Translation followed by rotation", 332 | ) 333 | 334 | # Test 3: Complex chain 335 | point = np.array([1, 0, 0]) 336 | transforms = [ 337 | {"translation": np.array([1, 1, 1]), "rotation": {"x": 90}}, 338 | {"translation": np.array([0, 0, 1]), "rotation": {"y": 90}}, 339 | {"translation": np.array([1, 0, 0]), "rotation": {"z": 90}}, 340 | ] 341 | transformed = chain_transforms(point, transforms) 342 | self.assert_array_almost_equal( 343 | transformed, 344 | [0, 1, -1], 345 | decimal=3, 346 | message="Complex chain of transformations", 347 | ) 348 | 349 | # Test 4: Identity chain 350 | point = np.array([1, 2, 3]) 351 | transforms = [ 352 | {"translation": np.array([0, 0, 0]), "rotation": {"x": 0}}, 353 | {"translation": np.array([0, 0, 0]), "rotation": {"y": 0}}, 354 | {"translation": np.array([0, 0, 0]), "rotation": {"z": 0}}, 355 | ] 356 | transformed = chain_transforms(point, transforms) 357 | self.assert_array_almost_equal( 358 | transformed, point, message="Chain of identity transformations" 359 | ) 360 | 361 | # Test 5: Complex chains 362 | point = np.array([1, 2, 3]) 363 | transforms = [ 364 | { 365 | "translation": np.array([1, 0, -1]), 366 | "rotation": {"x": 45, "y": 30, "z": 60}, 367 | }, 368 | { 369 | "translation": np.array([-1, 1, 0]), 370 | "rotation": {"x": 135, "y": 120, "z": 30}, 371 | }, 372 | { 373 | "translation": np.array([0, -1, 1]), 374 | "rotation": {"x": -90, "y": 60, "z": 0}, 375 | }, 376 | ] 377 | transformed = chain_transforms(point, transforms) 378 | self.assert_array_almost_equal( 379 | transformed, 380 | [1.8308927, -0.38874408, -2.0454028], 381 | decimal=3, 382 | message="Complex transformations", 383 | ) 384 | 385 | # Test 6: 386 | point = np.array([1, 1, 1]) 387 | transforms = [ 388 | { 389 | "translation": np.array([1, 1, 1]), 390 | "rotation": {"x": 45, "y": 45, "z": 45}, 391 | }, 392 | { 393 | "translation": np.array([-1, -1, -1]), 394 | "rotation": {"x": -45, "y": -45, "z": -45}, 395 | }, 396 | ] 397 | transformed = chain_transforms(point, transforms) 398 | self.assert_array_almost_equal( 399 | transformed, 400 | [-1.16421356, 1.28033009, 1.48743687], 401 | decimal=4, 402 | message="Tranform * Reverse transform", 403 | ) 404 | 405 | # Test 6: Complex chains 406 | point = np.array([1, 2, 3]) 407 | transforms = [ 408 | { 409 | "translation": np.array([1, 0, -1]), 410 | "rotation": {"x": 45, "y": 30, "z": 60}, 411 | }, 412 | { 413 | "translation": np.array([-1, 1, 0]), 414 | "rotation": {"x": 135, "y": 120, "z": 30}, 415 | }, 416 | { 417 | "translation": np.array([0, -1, 1]), 418 | "rotation": {"x": -90, "y": 60, "z": 0}, 419 | }, 420 | { 421 | "translation": np.array([1, 1, 1]), 422 | "rotation": {"x": 90, "y": 90, "z": 90}, 423 | }, 424 | { 425 | "translation": np.array([1, 1, 0]), 426 | "rotation": {"x": 135, "y": 120, "z": 30}, 427 | }, 428 | { 429 | "translation": np.array([0, 1, -1]), 430 | "rotation": {"x": -90, "y": 60, "z": 0}, 431 | }, 432 | ] 433 | transformed = chain_transforms(point, transforms) 434 | self.assert_array_almost_equal( 435 | transformed, 436 | [2.56055, -2.43085, -1.54043], 437 | decimal=3, 438 | message="Complex transformations with additional transform", 439 | ) 440 | 441 | # Test 7: Complex chains 442 | point = np.array([4, 5, 6]) 443 | transforms = [ 444 | { 445 | "translation": np.array([1, 0, -1]), 446 | "rotation": {"x": 150, "y": 130, "z": 220}, 447 | }, 448 | { 449 | "translation": np.array([-1, 1, 0]), 450 | "rotation": {"x": 135, "y": 120, "z": 90}, 451 | }, 452 | { 453 | "translation": np.array([0, -1, 1]), 454 | "rotation": {"x": 120, "y": 270, "z": 110}, 455 | }, 456 | ] 457 | transformed = chain_transforms(point, transforms) 458 | self.assert_array_almost_equal( 459 | transformed, 460 | [0.99092, -9.01667, 1.72435], 461 | decimal=3, 462 | message="Complex transformations with big angles", 463 | ) 464 | 465 | # Test 8: Complex chains 466 | point = np.array([3, 6, 9]) 467 | transforms = [ 468 | { 469 | "translation": np.array([1, 0, -1]), 470 | "rotation": {"x": -45, "y": 30, "z": -60}, 471 | }, 472 | { 473 | "translation": np.array([-1, 1, 0]), 474 | "rotation": {"x": -60, "y": -30, "z": -90}, 475 | }, 476 | { 477 | "translation": np.array([0, -1, 1]), 478 | "rotation": {"x": -45, "y": 30, "z": -30}, 479 | }, 480 | { 481 | "translation": np.array([1, 1, 1]), 482 | "rotation": {"x": 45, "y": -60, "z": -45}, 483 | }, 484 | { 485 | "translation": np.array([1, 1, 0]), 486 | "rotation": {"x": -30, "y": 60, "z": 30}, 487 | }, 488 | { 489 | "translation": np.array([0, 1, -1]), 490 | "rotation": {"x": -45, "y": 30, "z": -30}, 491 | }, 492 | ] 493 | transformed = chain_transforms(point, transforms) 494 | self.assert_array_almost_equal( 495 | transformed, 496 | [2.56055, -2.43085, -1.54043], 497 | decimal=3, 498 | message="Complex transformations with nagative angles and additional transform", 499 | ) 500 | 501 | # Test 9: Chains of Rotations Only 502 | point = np.array([3, 2, 4]) 503 | transforms = [ 504 | {'translation': np.array([0, 0, 0]), 'rotation': {'x': 0, 'y': 45, 'z': 45}}, 505 | {'translation': np.array([0, 0, 0]), 'rotation': {'x': 45, 'y': 45, 'z': 0}}, 506 | ] 507 | transformed = chain_transforms(point, transforms) 508 | self.assert_array_almost_equal( 509 | transformed, [4, 3, 2], 510 | message="Chain of rotations" 511 | ) 512 | 513 | # Test 10: Opposite transformations 514 | point = np.array([1, 1, 1]) 515 | transforms = [ 516 | {'translation': np.array([2, 2, 2]), 'rotation': {'x': 90, 'y': 90, 'z': 90}}, 517 | {'translation': np.array([-2, -2, -2]), 'rotation': {'x': -90, 'y': -90, 'z': -90}}, 518 | ] 519 | transformed = chain_transforms(point, transforms) 520 | self.assert_array_almost_equal( 521 | transformed, [-5, -1, 1], 522 | message="Opposite transformations" 523 | ) 524 | 525 | 526 | def print_summary(self): 527 | """Print summary of test results""" 528 | print(f"\n=== Test Summary ===") 529 | print(f"Passed: {self.passed_tests}/{self.total_tests} tests") 530 | if self.passed_tests == self.total_tests: 531 | print("🎉 All tests passed!") 532 | else: 533 | print(f"❌ {self.total_tests - self.passed_tests} tests failed") 534 | --------------------------------------------------------------------------------