Add simulator examples and fix dynamics runtime issue

examples/README.md

@@ -0,0 +1,17 @@
+# Simulator Examples
+
+This folder contains runnable examples for the robot simulator.
+
+## TwoLink Demo
+```bash
+uv run python examples/twolink_demo.py
+```
+## Tree7 Demo
+```bash
+uv run python examples/tree7_demo.py
+```
+## Random Tree Demo
+```bash
+uv run python examples/random_tree_demo.py
+```
+

examples/random_tree_demo.py

@@ -0,0 +1,31 @@
+import sys
+from pathlib import Path
+
+sys.path.append(str(Path(__file__).resolve().parents[1] / "src" / "simulator"))
+
+from physics import PhysicsEngine
+from renderer import Renderer
+from world import World
+
+from objects import RobotTree
+from dynamics.ab_algorithm import ABAlgorithm
+
+
+def main():
+    fd_solver = ABAlgorithm()
+
+    physics = PhysicsEngine(fd_solver, gravity=[0.0, -9.81, 0.0])
+    renderer = Renderer()
+
+    world = World(physics, renderer)
+
+    robot = RobotTree()
+    robot.some_tree(8, 2)
+
+    world.add_object(robot)
+
+    world.run(1000)
+
+
+if __name__ == "__main__":
+    main()

examples/tree7_demo.py

@@ -0,0 +1,31 @@
+import sys
+from pathlib import Path
+
+sys.path.append(str(Path(__file__).resolve().parents[1] / "src" / "simulator"))
+
+from physics import PhysicsEngine
+from renderer import Renderer
+from world import World
+
+from objects import Tree7
+from dynamics.ab_algorithm import ABAlgorithm
+
+
+def main():
+    fd_solver = ABAlgorithm()
+
+    physics = PhysicsEngine(fd_solver, gravity=[0.0, -9.81, 0.0])
+    renderer = Renderer()
+
+    world = World(physics, renderer)
+
+    robot = Tree7()
+
+    world.add_object(robot)
+
+    world.run(1000)
+
+
+if __name__ == "__main__":
+    main()
+1

examples/twolink_demo.py

@@ -0,0 +1,31 @@
+import sys
+from pathlib import Path
+
+sys.path.append(str(Path(__file__).resolve().parents[1] / "src" / "simulator"))
+
+from physics import PhysicsEngine
+from renderer import Renderer
+from world import World
+
+from objects import TwoLink
+from dynamics.ab_algorithm import ABAlgorithm
+
+
+def main():
+    fd_solver = ABAlgorithm()
+
+    physics = PhysicsEngine(fd_solver, gravity=[0.0, -9.81, 0.0])
+    renderer = Renderer()
+
+    world = World(physics, renderer)
+
+    robot = TwoLink()
+
+    world.add_object(robot)
+
+    world.run(1000)
+
+
+if __name__ == "__main__":
+    main()
+1

pyproject.toml

@@ -5,10 +5,12 @@ description = "Add your description here"
 readme = "README.md"
 requires-python = ">=3.12"
 dependencies = [
+    "matplotlib>=3.10.9",
     "numpy>=2.4.4",
     "pre-commit>=4.6.0",
     "pytest>=9.0.3",
     "ruff>=0.15.12",
+    "scipy>=1.17.1",
 ]
 
 [tool.ruff.lint]

src/simulator/dynamics/ab_algorithm.py

@@ -65,7 +65,7 @@ def forward_dynamics(self, obj, q, qd, tau, f_ext=None):
                 a[i] = Xup[i] @ (-a_grav) + c[i]
             else:
                 a[i] = Xup[i] @ a[parent] + c[i]
-            qdd[i] = (u[i] - U[i].T @ a[i]) / d[i]
+            qdd[i] = ((u[i] - U[i].T @ a[i]) / d[i]).item()
             a[i] = a[i] + S[i].dot(qdd[i])
 
         return qdd
@@ -80,9 +80,18 @@ def apply_external_forces(self, parent, Xup, pA, f_ext):
 
     def jcalc(self, jtyp, q):
         joint_map = {
-            "Rx": (lambda q: SE3(SO3.rx(q).T).adjoint(), np.array([[1], [0], [0], [0], [0], [0]])),
-            "Ry": (lambda q: SE3(SO3.ry(q).T).adjoint(), np.array([[0], [1], [0], [0], [0], [0]])),
-            "Rz": (lambda q: SE3(SO3.rz(q).T).adjoint(), np.array([[0], [0], [1], [0], [0], [0]])),
+            "Rx": (
+                lambda q: SE3(SO3.rx(q).T).adjoint(),
+                np.array([[1], [0], [0], [0], [0], [0]]),
+            ),
+            "Ry": (
+                lambda q: SE3(SO3.ry(q).T).adjoint(),
+                np.array([[0], [1], [0], [0], [0], [0]]),
+            ),
+            "Rz": (
+                lambda q: SE3(SO3.rz(q).T).adjoint(),
+                np.array([[0], [0], [1], [0], [0], [0]]),
+            ),
             "Px": (
                 lambda q: SE3(SO3().matrix, [q, 0, 0]).adjoint(),
                 np.array([[0], [0], [0], [1], [0], [0]]),