dynamicalnodes 0.1.2__tar.gz → 0.1.3__tar.gz
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- {dynamicalnodes-0.1.2/src/dynamicalnodes.egg-info → dynamicalnodes-0.1.3}/PKG-INFO +8 -8
- dynamicalnodes-0.1.3/README.md +38 -0
- {dynamicalnodes-0.1.2 → dynamicalnodes-0.1.3}/pyproject.toml +1 -1
- dynamicalnodes-0.1.3/src/dynamicalnodes/dynamical_system.py +204 -0
- {dynamicalnodes-0.1.2 → dynamicalnodes-0.1.3}/src/dynamicalnodes/rosnode.py +58 -96
- {dynamicalnodes-0.1.2 → dynamicalnodes-0.1.3/src/dynamicalnodes.egg-info}/PKG-INFO +8 -8
- dynamicalnodes-0.1.2/README.md +0 -38
- dynamicalnodes-0.1.2/src/dynamicalnodes/dynamical_system.py +0 -226
- {dynamicalnodes-0.1.2 → dynamicalnodes-0.1.3}/LICENSE +0 -0
- {dynamicalnodes-0.1.2 → dynamicalnodes-0.1.3}/setup.cfg +0 -0
- {dynamicalnodes-0.1.2 → dynamicalnodes-0.1.3}/src/dynamicalnodes/__init__.py +0 -0
- {dynamicalnodes-0.1.2 → dynamicalnodes-0.1.3}/src/dynamicalnodes/ros2py_py2ros.py +0 -0
- {dynamicalnodes-0.1.2 → dynamicalnodes-0.1.3}/src/dynamicalnodes.egg-info/SOURCES.txt +0 -0
- {dynamicalnodes-0.1.2 → dynamicalnodes-0.1.3}/src/dynamicalnodes.egg-info/dependency_links.txt +0 -0
- {dynamicalnodes-0.1.2 → dynamicalnodes-0.1.3}/src/dynamicalnodes.egg-info/requires.txt +0 -0
- {dynamicalnodes-0.1.2 → dynamicalnodes-0.1.3}/src/dynamicalnodes.egg-info/top_level.txt +0 -0
|
@@ -1,6 +1,6 @@
|
|
|
1
1
|
Metadata-Version: 2.4
|
|
2
2
|
Name: dynamicalnodes
|
|
3
|
-
Version: 0.1.
|
|
3
|
+
Version: 0.1.3
|
|
4
4
|
Summary: dynamicalnodes is a Python library for modeling general control systems.
|
|
5
5
|
Author-email: Nehal Singh Mangat <nehalsinghmangat.software@gmail.com>
|
|
6
6
|
License-Expression: MIT
|
|
@@ -44,33 +44,33 @@ Dynamic: license-file
|
|
|
44
44
|
|
|
45
45
|
> _"Give me a [control system] long enough and a [computer] on which to place it, and I shall move the world."_ - Archimedes
|
|
46
46
|
|
|
47
|
-
**dynamicalnodes** is a Python development framework that bridges the gap between control systems and their implementation onto ROS 2. Designed for hobbyists, students, and academics alike, this framework won't cure cancer, but it can do the next best thing: make robotics easier.
|
|
47
|
+
**dynamicalnodes** is a Python development framework that bridges the gap between novel control systems and their implementation onto ROS 2. Designed for hobbyists, students, and academics alike, this framework won't cure cancer, but it can do the next best thing: make robotics easier.
|
|
48
48
|
|
|
49
49
|
To get started, or to explore what this framework has to offer, click here: [dynamicalnodes Documentation](https://nehalsinghmangat.github.io/dynamicalnodes).
|
|
50
50
|
|
|
51
51
|
For a 22 second video describing this framework's intended workflow for, click here: [SAIL 2025 -- Presenting dynamicalnodes](https://youtu.be/5GbVHo6QZrw).
|
|
52
52
|
|
|
53
53
|
---
|
|
54
|
-
## Turtlebot: Whiteboard
|
|
54
|
+
## Turtlebot: Whiteboard ⟶ Python ⟶ ROS 2 ⟶ World
|
|
55
55
|
<table>
|
|
56
56
|
<tr>
|
|
57
57
|
<td align="center">
|
|
58
|
-
<img src="docs/source/_static/
|
|
58
|
+
<img src="docs/source/_static/figures/turtlebot_schematic.svg" alt="TurtleBot Schematic" width="400"/><br/>
|
|
59
59
|
</td>
|
|
60
60
|
<td align="center">
|
|
61
|
-
<img src="docs/source/_static/
|
|
61
|
+
<img src="docs/source/_static/figures/turtlebot_python_simulation.gif" alt="Turtle Software" width="200"/><br/>
|
|
62
62
|
</td>
|
|
63
63
|
<td align="center">
|
|
64
|
-
<img src="docs/source/_static/
|
|
64
|
+
<img src="docs/source/_static/figures/turtlebot_ros_simulation.gif" alt="Turtle Simulation" width="200"/><br/>
|
|
65
65
|
</td>
|
|
66
66
|
<td align="center">
|
|
67
|
-
<img src="docs/source/_static/
|
|
67
|
+
<img src="docs/source/_static/figures/turtlebot_mocab.gif" alt="Turtle Hardware" width="200"/><br/>
|
|
68
68
|
</td>
|
|
69
69
|
</tr>
|
|
70
70
|
</table>
|
|
71
71
|
|
|
72
72
|
<p align="center">
|
|
73
|
-
<img src="docs/source/_static/
|
|
73
|
+
<img src="docs/source/_static/figures/pipeline_theory_python_ros_hardware.png" alt="Step Labels" width="800"/>
|
|
74
74
|
</p>
|
|
75
75
|
|
|
76
76
|
|
|
@@ -0,0 +1,38 @@
|
|
|
1
|
+
# dynamicalnodes: Control Systems on ROS 2
|
|
2
|
+
|
|
3
|
+
> _"Give me a [control system] long enough and a [computer] on which to place it, and I shall move the world."_ - Archimedes
|
|
4
|
+
|
|
5
|
+
**dynamicalnodes** is a Python development framework that bridges the gap between novel control systems and their implementation onto ROS 2. Designed for hobbyists, students, and academics alike, this framework won't cure cancer, but it can do the next best thing: make robotics easier.
|
|
6
|
+
|
|
7
|
+
To get started, or to explore what this framework has to offer, click here: [dynamicalnodes Documentation](https://nehalsinghmangat.github.io/dynamicalnodes).
|
|
8
|
+
|
|
9
|
+
For a 22 second video describing this framework's intended workflow for, click here: [SAIL 2025 -- Presenting dynamicalnodes](https://youtu.be/5GbVHo6QZrw).
|
|
10
|
+
|
|
11
|
+
---
|
|
12
|
+
## Turtlebot: Whiteboard ⟶ Python ⟶ ROS 2 ⟶ World
|
|
13
|
+
<table>
|
|
14
|
+
<tr>
|
|
15
|
+
<td align="center">
|
|
16
|
+
<img src="docs/source/_static/figures/turtlebot_schematic.svg" alt="TurtleBot Schematic" width="400"/><br/>
|
|
17
|
+
</td>
|
|
18
|
+
<td align="center">
|
|
19
|
+
<img src="docs/source/_static/figures/turtlebot_python_simulation.gif" alt="Turtle Software" width="200"/><br/>
|
|
20
|
+
</td>
|
|
21
|
+
<td align="center">
|
|
22
|
+
<img src="docs/source/_static/figures/turtlebot_ros_simulation.gif" alt="Turtle Simulation" width="200"/><br/>
|
|
23
|
+
</td>
|
|
24
|
+
<td align="center">
|
|
25
|
+
<img src="docs/source/_static/figures/turtlebot_mocab.gif" alt="Turtle Hardware" width="200"/><br/>
|
|
26
|
+
</td>
|
|
27
|
+
</tr>
|
|
28
|
+
</table>
|
|
29
|
+
|
|
30
|
+
<p align="center">
|
|
31
|
+
<img src="docs/source/_static/figures/pipeline_theory_python_ros_hardware.png" alt="Step Labels" width="800"/>
|
|
32
|
+
</p>
|
|
33
|
+
|
|
34
|
+
|
|
35
|
+
|
|
36
|
+
|
|
37
|
+
|
|
38
|
+
|
|
@@ -7,7 +7,7 @@ build-backend = "setuptools.build_meta"
|
|
|
7
7
|
|
|
8
8
|
[project]
|
|
9
9
|
name = "dynamicalnodes"
|
|
10
|
-
version = "0.1.
|
|
10
|
+
version = "0.1.3"
|
|
11
11
|
description = "dynamicalnodes is a Python library for modeling general control systems."
|
|
12
12
|
readme = "README.md"
|
|
13
13
|
requires-python = ">=3.12"
|
|
@@ -0,0 +1,204 @@
|
|
|
1
|
+
"""
|
|
2
|
+
A discrete-time dynamical system (DTDS) is represented as a pair of functions,
|
|
3
|
+
``(f, h)``, following the definition in Mangat et al., *dynamicalnodes: From
|
|
4
|
+
Theory to Python to ROS*::
|
|
5
|
+
|
|
6
|
+
x_{k+1} = f(x_k, u_k, theta)
|
|
7
|
+
y_k = h(x_k, u_k, theta)
|
|
8
|
+
|
|
9
|
+
``f`` is the evolution function that propagates the state forward one step;
|
|
10
|
+
``h`` is the measurement (or output) function that computes an observation
|
|
11
|
+
from the current state. Either function may be omitted -- denoted ``*`` in
|
|
12
|
+
the paper, ``None`` here -- giving three cases:
|
|
13
|
+
|
|
14
|
+
- ``(f, *)`` -- no output function.
|
|
15
|
+
- ``(*, h)`` -- stateless, e.g. a signal generator.
|
|
16
|
+
- ``(f, h)`` -- both state evolution and output are defined.
|
|
17
|
+
|
|
18
|
+
Function signatures are unconstrained in name, type, and number of
|
|
19
|
+
parameters. ``DynamicalSystem.eval`` binds keyword arguments to each
|
|
20
|
+
function's signature by name, so the same keyword argument (e.g. a shared
|
|
21
|
+
state or parameter) can be passed to both ``f`` and ``h``.
|
|
22
|
+
"""
|
|
23
|
+
|
|
24
|
+
import inspect
|
|
25
|
+
from typing import Any, Callable, Dict, Optional, Tuple
|
|
26
|
+
|
|
27
|
+
|
|
28
|
+
class DynamicalSystem:
|
|
29
|
+
"""
|
|
30
|
+
A discrete-time dynamical system with (optional) evolution and
|
|
31
|
+
(optional) measurement functions.
|
|
32
|
+
|
|
33
|
+
Parameters
|
|
34
|
+
----------
|
|
35
|
+
f : Callable, optional
|
|
36
|
+
Evolution function ``f(x_k, u_k, theta) -> x_{k+1}``. ``None`` if
|
|
37
|
+
the system has no state transition, i.e. ``(*, h)``.
|
|
38
|
+
h : Callable, optional
|
|
39
|
+
Measurement function ``h(x_k, u_k, theta) -> y_k``. ``None`` if
|
|
40
|
+
the system has no output, i.e. ``(f, *)``.
|
|
41
|
+
|
|
42
|
+
Examples
|
|
43
|
+
--------
|
|
44
|
+
A stateful system with both an evolution and a measurement function --
|
|
45
|
+
a car whose state is ``(displacement, velocity)``, measured via its
|
|
46
|
+
speed:
|
|
47
|
+
|
|
48
|
+
>>> def fcar(xk, uk, theta_car):
|
|
49
|
+
... d, v = xk
|
|
50
|
+
... m, b, dt = theta_car
|
|
51
|
+
... return (d + dt * v, v + dt * (uk - b * v) / m)
|
|
52
|
+
>>> def hcar(xk):
|
|
53
|
+
... _, v = xk
|
|
54
|
+
... return abs(v)
|
|
55
|
+
>>> car = DynamicalSystem(f=fcar, h=hcar)
|
|
56
|
+
>>> x_next, y = car.eval(xk=(20.0, 0.0), uk=4.0, theta_car=(2.0, 0.0, 1.0))
|
|
57
|
+
>>> x_next
|
|
58
|
+
(20.0, 2.0)
|
|
59
|
+
>>> y
|
|
60
|
+
0.0
|
|
61
|
+
|
|
62
|
+
A stateless system, e.g. a signal generator -- ``(*, h)``:
|
|
63
|
+
|
|
64
|
+
>>> def h_ref(tk, A=1.0):
|
|
65
|
+
... return A if tk >= 1.0 else 0.0
|
|
66
|
+
>>> ref = DynamicalSystem(h=h_ref)
|
|
67
|
+
>>> ref.eval(tk=0.0)
|
|
68
|
+
(None, 0.0)
|
|
69
|
+
>>> ref.eval(tk=2.0)
|
|
70
|
+
(None, 1.0)
|
|
71
|
+
|
|
72
|
+
A system with no output function -- ``(f, *)``:
|
|
73
|
+
|
|
74
|
+
>>> def f_counter(xk):
|
|
75
|
+
... return xk + 1
|
|
76
|
+
>>> counter = DynamicalSystem(f=f_counter)
|
|
77
|
+
>>> counter.eval(xk=5)
|
|
78
|
+
(6, None)
|
|
79
|
+
"""
|
|
80
|
+
|
|
81
|
+
def __init__(
|
|
82
|
+
self,
|
|
83
|
+
*,
|
|
84
|
+
f: Optional[Callable] = None,
|
|
85
|
+
h: Optional[Callable] = None,
|
|
86
|
+
) -> None:
|
|
87
|
+
"""
|
|
88
|
+
Parameters
|
|
89
|
+
----------
|
|
90
|
+
f : Callable, optional
|
|
91
|
+
Evolution function ``f(x_k, u_k, theta) -> x_{k+1}``. Signature
|
|
92
|
+
typing is not enforced. ``None`` if the system is stateless.
|
|
93
|
+
h : Callable, optional
|
|
94
|
+
Measurement function ``h(x_k, u_k, theta) -> y_k``. Signature
|
|
95
|
+
typing is not enforced. ``None`` if the system has no output.
|
|
96
|
+
"""
|
|
97
|
+
self._f = f
|
|
98
|
+
self._h = h
|
|
99
|
+
|
|
100
|
+
@property
|
|
101
|
+
def f(self) -> Optional[Callable]:
|
|
102
|
+
"""Evolution function f(x_k, u_k, theta) -> x_{k+1}, or None if the system is stateless."""
|
|
103
|
+
return self._f
|
|
104
|
+
|
|
105
|
+
@f.setter
|
|
106
|
+
def f(self, f: Optional[Callable]) -> None:
|
|
107
|
+
self._f = f
|
|
108
|
+
|
|
109
|
+
@property
|
|
110
|
+
def h(self) -> Optional[Callable]:
|
|
111
|
+
"""Measurement function h(x_k, u_k, theta) -> y_k, or None if the system has no output."""
|
|
112
|
+
return self._h
|
|
113
|
+
|
|
114
|
+
@h.setter
|
|
115
|
+
def h(self, h: Optional[Callable]) -> None:
|
|
116
|
+
self._h = h
|
|
117
|
+
|
|
118
|
+
def eval(self, **kwargs) -> Tuple[Optional[Any], Optional[Any]]:
|
|
119
|
+
"""
|
|
120
|
+
Evaluate the system for one discrete time step.
|
|
121
|
+
|
|
122
|
+
Keyword arguments are lexically bound by name to whichever of ``f``
|
|
123
|
+
and/or ``h`` declare them in their signature (see
|
|
124
|
+
``_bind_and_call``), so the same keyword argument can be shared by
|
|
125
|
+
both functions.
|
|
126
|
+
|
|
127
|
+
Returns
|
|
128
|
+
-------
|
|
129
|
+
Tuple[Optional[Any], Optional[Any]]
|
|
130
|
+
- ``(None, y_k)`` for a stateless system, ``(*, h)``.
|
|
131
|
+
- ``(x_{k+1}, None)`` for a system with no output, ``(f, *)``.
|
|
132
|
+
- ``(x_{k+1}, y_k)`` for a fully specified system, ``(f, h)``.
|
|
133
|
+
|
|
134
|
+
Notes
|
|
135
|
+
-----
|
|
136
|
+
``h`` is evaluated on the same (pre-update) state passed in via
|
|
137
|
+
keyword arguments, not on the value returned by ``f``, so the
|
|
138
|
+
returned observation is one step behind the returned state. In a
|
|
139
|
+
simulation loop, use the returned ``x_{k+1}`` directly if you need
|
|
140
|
+
the post-update state.
|
|
141
|
+
|
|
142
|
+
Raises
|
|
143
|
+
------
|
|
144
|
+
ValueError
|
|
145
|
+
If neither ``f`` nor ``h`` is defined.
|
|
146
|
+
|
|
147
|
+
Examples
|
|
148
|
+
--------
|
|
149
|
+
See the class docstring for worked examples covering all three
|
|
150
|
+
cases.
|
|
151
|
+
|
|
152
|
+
>>> empty = DynamicalSystem()
|
|
153
|
+
>>> empty.eval()
|
|
154
|
+
Traceback (most recent call last):
|
|
155
|
+
...
|
|
156
|
+
ValueError: Nothing to evaluate; no f or h functions!
|
|
157
|
+
"""
|
|
158
|
+
|
|
159
|
+
def _bind_and_call(
|
|
160
|
+
func: Callable[..., Any],
|
|
161
|
+
**kwargs: Any,
|
|
162
|
+
) -> Any:
|
|
163
|
+
"""
|
|
164
|
+
Call `func` with only the keyword arguments it declares.
|
|
165
|
+
|
|
166
|
+
Inspects `func`'s signature and passes only the entries of
|
|
167
|
+
`kwargs` matching its declared parameter names; if `func`
|
|
168
|
+
accepts `**kwargs`, the remaining pool is passed through too.
|
|
169
|
+
"""
|
|
170
|
+
sig = inspect.signature(func)
|
|
171
|
+
pool: Dict[str, Any] = dict(kwargs)
|
|
172
|
+
|
|
173
|
+
# Filter: only pass arguments the function declares
|
|
174
|
+
call_kwargs: Dict[str, Any] = {}
|
|
175
|
+
for name, p in sig.parameters.items():
|
|
176
|
+
if p.kind in (
|
|
177
|
+
inspect.Parameter.POSITIONAL_OR_KEYWORD,
|
|
178
|
+
inspect.Parameter.KEYWORD_ONLY,
|
|
179
|
+
):
|
|
180
|
+
if name in pool:
|
|
181
|
+
call_kwargs[name] = pool.pop(name)
|
|
182
|
+
|
|
183
|
+
# Pass remaining kwargs if function has **kwargs
|
|
184
|
+
if any(
|
|
185
|
+
p.kind is inspect.Parameter.VAR_KEYWORD for p in sig.parameters.values()
|
|
186
|
+
):
|
|
187
|
+
call_kwargs.update(pool)
|
|
188
|
+
|
|
189
|
+
return func(**call_kwargs)
|
|
190
|
+
|
|
191
|
+
if self.f is None and self.h:
|
|
192
|
+
return (None, _bind_and_call(self.h, **kwargs))
|
|
193
|
+
|
|
194
|
+
elif self.f and self.h is None:
|
|
195
|
+
return (_bind_and_call(self.f, **kwargs), None)
|
|
196
|
+
|
|
197
|
+
elif self.f and self.h:
|
|
198
|
+
return (
|
|
199
|
+
_bind_and_call(self.f, **kwargs),
|
|
200
|
+
_bind_and_call(self.h, **kwargs),
|
|
201
|
+
)
|
|
202
|
+
|
|
203
|
+
else:
|
|
204
|
+
raise ValueError("Nothing to evaluate; no f or h functions!")
|
|
@@ -3,57 +3,14 @@ ROSNode wrapper for simulation and ROS2 deployment.
|
|
|
3
3
|
|
|
4
4
|
Two workflows:
|
|
5
5
|
|
|
6
|
-
**Simulation** — call ``
|
|
6
|
+
**Simulation** — call ``eval()`` directly in Python or Jupyter notebooks to test ros2py and/or py2ros translation functions.
|
|
7
7
|
No ROS2 installation required.
|
|
8
8
|
|
|
9
9
|
**Deployment** — call ``write_ROSNode_to_rclpy(path)`` to emit a self-contained
|
|
10
10
|
``.py`` file that spins up a native rclpy node. Every ROS2 detail (QoS profiles,
|
|
11
|
-
buffer depths, subscription callbacks, executor setup) is written out explicitly
|
|
12
|
-
|
|
13
|
-
|
|
14
|
-
|
|
15
|
-
Example
|
|
16
|
-
-------
|
|
17
|
-
Develop and simulate in a notebook, then deploy:
|
|
18
|
-
|
|
19
|
-
>>> import numpy as np
|
|
20
|
-
>>> from dynamicalnodes import DynamicalSystem, ROSNode
|
|
21
|
-
>>> from std_msgs.msg import Float64
|
|
22
|
-
>>> from dynamicalnodes.ros2py_py2ros import ros2py_float64, py2ros_float64
|
|
23
|
-
>>>
|
|
24
|
-
>>> def f_plant(xk, u_k):
|
|
25
|
-
... return xk + 0.1 * u_k
|
|
26
|
-
>>> def h_plant(xk):
|
|
27
|
-
... return xk
|
|
28
|
-
>>>
|
|
29
|
-
>>> sys = DynamicalSystem(f=f_plant, h=h_plant)
|
|
30
|
-
>>> node = ROSNode(
|
|
31
|
-
... dynamical_system=sys,
|
|
32
|
-
... subscribes_to=[
|
|
33
|
-
... {"topic": "/u_k", "msg_type": Float64, "arg": "u_k",
|
|
34
|
-
... "ros2py": ros2py_float64, "stale_after": 0.5},
|
|
35
|
-
... ],
|
|
36
|
-
... publishes_to=[
|
|
37
|
-
... {"topic": "/y_k", "msg_type": Float64, "py2ros": py2ros_float64},
|
|
38
|
-
... ],
|
|
39
|
-
... state_name="xk",
|
|
40
|
-
... )
|
|
41
|
-
>>>
|
|
42
|
-
>>> # Simulate
|
|
43
|
-
>>> node._state = np.array([0.0])
|
|
44
|
-
>>> for k in range(3):
|
|
45
|
-
... y = node.step(u_k=np.array([1.0]))
|
|
46
|
-
... print(f"k={k}: x={node.state}")
|
|
47
|
-
k=0: x=[0.1]
|
|
48
|
-
k=1: x=[0.2]
|
|
49
|
-
k=2: x=[0.3]
|
|
50
|
-
>>>
|
|
51
|
-
>>> # Deploy — writes a standalone rclpy node to disk
|
|
52
|
-
>>> import tempfile, os
|
|
53
|
-
>>> with tempfile.NamedTemporaryFile(suffix=".py", delete=False) as f:
|
|
54
|
-
... path = f.name
|
|
55
|
-
>>> node.write_ROSNode_to_rclpy(path, node_name="plant", initial_state=np.array([0.0]))
|
|
56
|
-
>>> os.unlink(path)
|
|
11
|
+
buffer depths, subscription callbacks, executor setup, etc...) is written out explicitly and
|
|
12
|
+
can be modified as needed. The ``.py`` file also has no runtime dependency on dynamicalnodes beyond ``DynamicalSystem`` and whatever dependencies the
|
|
13
|
+
f/h functions may have.
|
|
57
14
|
|
|
58
15
|
See Also
|
|
59
16
|
--------
|
|
@@ -79,21 +36,21 @@ class ROSNode:
|
|
|
79
36
|
"""
|
|
80
37
|
Wraps a DynamicalSystem for simulation and ROS2 deployment.
|
|
81
38
|
|
|
82
|
-
Use ``
|
|
39
|
+
Use ``eval()`` for notebook simulation; use ``write_ROSNode_to_rclpy()`` to
|
|
83
40
|
generate a deployable rclpy node.
|
|
84
41
|
|
|
85
42
|
Parameters
|
|
86
43
|
----------
|
|
87
44
|
dynamical_system : DynamicalSystem
|
|
88
|
-
The dynamical system to
|
|
89
|
-
|
|
45
|
+
The dynamical system to evaluate.
|
|
46
|
+
subs : list of dict, optional
|
|
90
47
|
Each dict:
|
|
91
48
|
|
|
92
49
|
- ``"topic"`` (str): ROS topic name.
|
|
93
50
|
- ``"msg_type"`` (type): ROS message class.
|
|
94
|
-
- ``"arg"`` (str): kwarg name passed to
|
|
51
|
+
- ``"arg"`` (str): kwarg name passed to eval().
|
|
95
52
|
- ``"ros2py"`` (Callable): ROS msg → NumPy array converter.
|
|
96
|
-
Import from ``dynamicalnodes.ros2py_py2ros
|
|
53
|
+
Import from ``dynamicalnodes.ros2py_py2ros`` or write your own.
|
|
97
54
|
- ``"stale_after"`` (float, optional): Seconds before data expires.
|
|
98
55
|
- ``"buffer_size"`` (int, optional): Queue depth (default 1).
|
|
99
56
|
- ``"use_msg_timestamp"`` (bool, optional): When ``True``, the
|
|
@@ -104,7 +61,7 @@ class ROSNode:
|
|
|
104
61
|
rather than network-arrival time (e.g. sensor fusion, bag
|
|
105
62
|
replay). Defaults to ``False``.
|
|
106
63
|
|
|
107
|
-
|
|
64
|
+
pubs : list of dict, optional
|
|
108
65
|
Each dict:
|
|
109
66
|
|
|
110
67
|
- ``"topic"`` (str): ROS topic name.
|
|
@@ -114,15 +71,15 @@ class ROSNode:
|
|
|
114
71
|
- ``"key"`` (str, optional): Key into h() return dict. Required when
|
|
115
72
|
h() returns a dict and there are multiple publishers.
|
|
116
73
|
|
|
117
|
-
sync_mode : str, required when ``
|
|
74
|
+
sync_mode : str, required when ``subs`` has more than one entry
|
|
118
75
|
``"any"`` — step whenever any subscription has fresh data.
|
|
119
76
|
``"all"`` — step only when every subscription has fresh data.
|
|
120
77
|
|
|
121
78
|
state_name : str, optional
|
|
122
79
|
Name of the state parameter in ``f``'s signature (e.g. ``"ck"`` for
|
|
123
80
|
``f(ck, ...)``) . Required for stateful systems; ignored if ``f`` is
|
|
124
|
-
``None``. ``
|
|
125
|
-
calling ``DynamicalSystem.
|
|
81
|
+
``None``. ``eval()`` injects ``self._state`` under this name before
|
|
82
|
+
calling ``DynamicalSystem.eval()``.
|
|
126
83
|
|
|
127
84
|
timer_hz : float, optional
|
|
128
85
|
In the generated node: fires ``_run_step`` at this frequency instead
|
|
@@ -140,12 +97,12 @@ class ROSNode:
|
|
|
140
97
|
>>> smoother = DynamicalSystem(h=lambda imu: imu * 0.5)
|
|
141
98
|
>>> node = ROSNode(
|
|
142
99
|
... dynamical_system=smoother,
|
|
143
|
-
...
|
|
144
|
-
...
|
|
145
|
-
...
|
|
146
|
-
...
|
|
100
|
+
... subs=[{"topic": "/raw", "msg_type": Float64,
|
|
101
|
+
... "arg": "imu", "ros2py": ros2py_float64}],
|
|
102
|
+
... pubs=[{"topic": "/smooth", "msg_type": Float64,
|
|
103
|
+
... "py2ros": py2ros_float64}],
|
|
147
104
|
... )
|
|
148
|
-
>>> out = node.
|
|
105
|
+
>>> out = node.eval(imu=np.array([2.0]))
|
|
149
106
|
>>> out.data
|
|
150
107
|
1.0
|
|
151
108
|
"""
|
|
@@ -155,15 +112,15 @@ class ROSNode:
|
|
|
155
112
|
*,
|
|
156
113
|
dynamical_system: DynamicalSystem,
|
|
157
114
|
state_name: Optional[str] = None,
|
|
158
|
-
|
|
159
|
-
|
|
115
|
+
subs: Optional[Sequence[SubDict]] = None,
|
|
116
|
+
pubs: Optional[Sequence[PubDict]] = None,
|
|
160
117
|
sync_mode: Optional[str] = None,
|
|
161
118
|
timer_hz: Optional[float] = None,
|
|
162
119
|
) -> None:
|
|
163
|
-
n_subs = len(
|
|
120
|
+
n_subs = len(subs) if subs else 0
|
|
164
121
|
if n_subs > 1 and sync_mode is None:
|
|
165
122
|
raise ValueError(
|
|
166
|
-
"sync_mode ('any' or 'all') is required when
|
|
123
|
+
"sync_mode ('any' or 'all') is required when subs has "
|
|
167
124
|
"more than one subscription"
|
|
168
125
|
)
|
|
169
126
|
sync_mode = sync_mode or "any"
|
|
@@ -180,7 +137,7 @@ class ROSNode:
|
|
|
180
137
|
|
|
181
138
|
# -------- Parse subscriptions --------
|
|
182
139
|
self._subs: List[Tuple[str, type, Any, str, Optional[float], int, bool]] = []
|
|
183
|
-
for sub_dict in
|
|
140
|
+
for sub_dict in subs or []:
|
|
184
141
|
if not isinstance(sub_dict, dict):
|
|
185
142
|
raise TypeError("Each subscription must be a dict")
|
|
186
143
|
|
|
@@ -233,7 +190,7 @@ class ROSNode:
|
|
|
233
190
|
|
|
234
191
|
# -------- Parse publications --------
|
|
235
192
|
self._pubs_cfg: List[Tuple[str, type, Any, Optional[str]]] = []
|
|
236
|
-
for pub_dict in
|
|
193
|
+
for pub_dict in pubs or []:
|
|
237
194
|
if not isinstance(pub_dict, dict):
|
|
238
195
|
raise TypeError("Each publication must be a dict")
|
|
239
196
|
|
|
@@ -265,7 +222,7 @@ class ROSNode:
|
|
|
265
222
|
# SIMULATION API
|
|
266
223
|
# --------------------------------------------------------------------------
|
|
267
224
|
|
|
268
|
-
def
|
|
225
|
+
def eval(self, **kwargs: Any) -> Any:
|
|
269
226
|
"""
|
|
270
227
|
Simulate one step without ROS2.
|
|
271
228
|
|
|
@@ -297,12 +254,12 @@ class ROSNode:
|
|
|
297
254
|
>>> double = DynamicalSystem(h=lambda x: x * 2)
|
|
298
255
|
>>> node = ROSNode(
|
|
299
256
|
... dynamical_system=double,
|
|
300
|
-
...
|
|
301
|
-
...
|
|
302
|
-
...
|
|
303
|
-
...
|
|
257
|
+
... subs=[{"topic": "/x", "msg_type": Float64,
|
|
258
|
+
... "arg": "x", "ros2py": ros2py_float64}],
|
|
259
|
+
... pubs=[{"topic": "/y", "msg_type": Float64,
|
|
260
|
+
... "py2ros": py2ros_float64}],
|
|
304
261
|
... )
|
|
305
|
-
>>> node.
|
|
262
|
+
>>> node.eval(x=Float64(data=3.0)).data
|
|
306
263
|
6.0
|
|
307
264
|
"""
|
|
308
265
|
sub_arg_names = {a for _, _, _, a, _, _, _ in self._subs}
|
|
@@ -328,11 +285,9 @@ class ROSNode:
|
|
|
328
285
|
if self._state is not None and self._state_key is not None:
|
|
329
286
|
step_kwargs[self._state_key] = self._state
|
|
330
287
|
|
|
331
|
-
|
|
288
|
+
x_next, yk = self._dynamical_system.eval(**step_kwargs)
|
|
332
289
|
if self._dynamical_system.f is not None:
|
|
333
|
-
self._state
|
|
334
|
-
else:
|
|
335
|
-
yk = result
|
|
290
|
+
self._state = x_next
|
|
336
291
|
|
|
337
292
|
if not self._pubs_cfg:
|
|
338
293
|
return yk
|
|
@@ -398,7 +353,7 @@ class ROSNode:
|
|
|
398
353
|
``__init__``. Supports NumPy arrays and any ``repr()``-able value.
|
|
399
354
|
static_params : dict, optional
|
|
400
355
|
Parameters baked into the node at generation time. Embedded as
|
|
401
|
-
``self._static_params = {...}`` and merged into every ``
|
|
356
|
+
``self._static_params = {...}`` and merged into every ``eval()``
|
|
402
357
|
call. Use for values that never change after deployment.
|
|
403
358
|
dynamic_params : dict, optional
|
|
404
359
|
Parameters declared on the ROS2 parameter server with their
|
|
@@ -446,7 +401,8 @@ class ROSNode:
|
|
|
446
401
|
|
|
447
402
|
Notes
|
|
448
403
|
-----
|
|
449
|
-
**Function resolution
|
|
404
|
+
**Function resolution**
|
|
405
|
+
|
|
450
406
|
Functions from ``dynamicalnodes.ros2py_py2ros`` are emitted as import
|
|
451
407
|
statements. Functions from any other importable module (not
|
|
452
408
|
``__main__`` or an IPython cell) are also imported by name. All other
|
|
@@ -468,11 +424,11 @@ class ROSNode:
|
|
|
468
424
|
>>> sys = DynamicalSystem(f=f_plant, h=h_plant)
|
|
469
425
|
>>> node = ROSNode(
|
|
470
426
|
... dynamical_system=sys,
|
|
471
|
-
...
|
|
472
|
-
...
|
|
473
|
-
...
|
|
474
|
-
...
|
|
475
|
-
...
|
|
427
|
+
... subs=[{"topic": "/u_k", "msg_type": Float64,
|
|
428
|
+
... "arg": "u_k", "ros2py": ros2py_float64,
|
|
429
|
+
... "stale_after": 0.5}],
|
|
430
|
+
... pubs=[{"topic": "/y_k", "msg_type": Float64,
|
|
431
|
+
... "py2ros": py2ros_float64}],
|
|
476
432
|
... )
|
|
477
433
|
>>> with tempfile.NamedTemporaryFile(suffix=".py", delete=False) as tmp:
|
|
478
434
|
... path = tmp.name
|
|
@@ -619,7 +575,7 @@ class ROSNode:
|
|
|
619
575
|
if not self._subs and self._timer_hz is None and self._pubs_cfg:
|
|
620
576
|
raise ValueError(
|
|
621
577
|
"Cannot write a node with no subscriptions and no timer_hz — "
|
|
622
|
-
"the generated node would never step. Add timer_hz or
|
|
578
|
+
"the generated node would never step. Add timer_hz or subs."
|
|
623
579
|
)
|
|
624
580
|
|
|
625
581
|
ds = self._dynamical_system
|
|
@@ -636,10 +592,10 @@ class ROSNode:
|
|
|
636
592
|
_valid_pub_topics = {t for t, *_ in self._pubs_cfg}
|
|
637
593
|
for t in _sub_noise:
|
|
638
594
|
if t not in _valid_sub_topics:
|
|
639
|
-
raise ValueError(f"sub_noise topic {t!r} not in
|
|
595
|
+
raise ValueError(f"sub_noise topic {t!r} not in subs")
|
|
640
596
|
for t in _pub_noise:
|
|
641
597
|
if t not in _valid_pub_topics:
|
|
642
|
-
raise ValueError(f"pub_noise topic {t!r} not in
|
|
598
|
+
raise ValueError(f"pub_noise topic {t!r} not in pubs")
|
|
643
599
|
|
|
644
600
|
# ── classify all functions ─────────────────────────────────────────────
|
|
645
601
|
|
|
@@ -984,7 +940,9 @@ class ROSNode:
|
|
|
984
940
|
)
|
|
985
941
|
if topic in _sub_noise:
|
|
986
942
|
std = _sub_noise[topic]
|
|
987
|
-
ln(
|
|
943
|
+
ln(
|
|
944
|
+
f" arr = arr + np.random.normal(0.0, {std!r}, arr.shape) # noise std={std}"
|
|
945
|
+
)
|
|
988
946
|
if use_msg_ts:
|
|
989
947
|
ln(
|
|
990
948
|
f" ts = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9"
|
|
@@ -1015,7 +973,7 @@ class ROSNode:
|
|
|
1015
973
|
ln(" # Control step")
|
|
1016
974
|
ln(" # " + "─" * 68)
|
|
1017
975
|
ln(" # Builds kwargs from static params, dynamic params, fresh subscription")
|
|
1018
|
-
ln(" # values, and current state, then calls DynamicalSystem.
|
|
976
|
+
ln(" # values, and current state, then calls DynamicalSystem.eval() and")
|
|
1019
977
|
ln(" # publishes. Skipped entirely when sync_mode conditions are not met.")
|
|
1020
978
|
blank()
|
|
1021
979
|
ln(" def _run_step(self) -> None:")
|
|
@@ -1069,11 +1027,9 @@ class ROSNode:
|
|
|
1069
1027
|
ln(f' kwargs["{self._state_key}"] = self._state')
|
|
1070
1028
|
|
|
1071
1029
|
blank()
|
|
1072
|
-
ln("
|
|
1030
|
+
ln(" x_next, yk = self._system.eval(**kwargs)")
|
|
1073
1031
|
ln(" if self._system.f is not None:")
|
|
1074
|
-
ln(" self._state
|
|
1075
|
-
ln(" else:")
|
|
1076
|
-
ln(" yk = result # stateless: output only")
|
|
1032
|
+
ln(" self._state = x_next # stateful: advance to next state")
|
|
1077
1033
|
|
|
1078
1034
|
if pubs:
|
|
1079
1035
|
blank()
|
|
@@ -1081,10 +1037,14 @@ class ROSNode:
|
|
|
1081
1037
|
py2ros_name = _ref_name(py2ros, cls)
|
|
1082
1038
|
if key is not None:
|
|
1083
1039
|
ln(f" if isinstance(yk, dict) and {key!r} in yk:")
|
|
1084
|
-
ln(
|
|
1040
|
+
ln(
|
|
1041
|
+
f" _arr = np.asarray(yk[{key!r}], dtype=float).ravel()"
|
|
1042
|
+
)
|
|
1085
1043
|
if topic in _pub_noise:
|
|
1086
1044
|
std = _pub_noise[topic]
|
|
1087
|
-
ln(
|
|
1045
|
+
ln(
|
|
1046
|
+
f" _arr = _arr + np.random.normal(0.0, {std!r}, _arr.shape) # noise std={std}"
|
|
1047
|
+
)
|
|
1088
1048
|
ln(f" msg = {py2ros_name}(_arr)")
|
|
1089
1049
|
ln(f" if hasattr(msg, 'header'):")
|
|
1090
1050
|
ln(
|
|
@@ -1095,7 +1055,9 @@ class ROSNode:
|
|
|
1095
1055
|
ln(f" _arr = np.asarray(yk, dtype=float).ravel()")
|
|
1096
1056
|
if topic in _pub_noise:
|
|
1097
1057
|
std = _pub_noise[topic]
|
|
1098
|
-
ln(
|
|
1058
|
+
ln(
|
|
1059
|
+
f" _arr = _arr + np.random.normal(0.0, {std!r}, _arr.shape) # noise std={std}"
|
|
1060
|
+
)
|
|
1099
1061
|
ln(f" msg = {py2ros_name}(_arr)")
|
|
1100
1062
|
ln(f" if hasattr(msg, 'header'):")
|
|
1101
1063
|
ln(
|
|
@@ -1,6 +1,6 @@
|
|
|
1
1
|
Metadata-Version: 2.4
|
|
2
2
|
Name: dynamicalnodes
|
|
3
|
-
Version: 0.1.
|
|
3
|
+
Version: 0.1.3
|
|
4
4
|
Summary: dynamicalnodes is a Python library for modeling general control systems.
|
|
5
5
|
Author-email: Nehal Singh Mangat <nehalsinghmangat.software@gmail.com>
|
|
6
6
|
License-Expression: MIT
|
|
@@ -44,33 +44,33 @@ Dynamic: license-file
|
|
|
44
44
|
|
|
45
45
|
> _"Give me a [control system] long enough and a [computer] on which to place it, and I shall move the world."_ - Archimedes
|
|
46
46
|
|
|
47
|
-
**dynamicalnodes** is a Python development framework that bridges the gap between control systems and their implementation onto ROS 2. Designed for hobbyists, students, and academics alike, this framework won't cure cancer, but it can do the next best thing: make robotics easier.
|
|
47
|
+
**dynamicalnodes** is a Python development framework that bridges the gap between novel control systems and their implementation onto ROS 2. Designed for hobbyists, students, and academics alike, this framework won't cure cancer, but it can do the next best thing: make robotics easier.
|
|
48
48
|
|
|
49
49
|
To get started, or to explore what this framework has to offer, click here: [dynamicalnodes Documentation](https://nehalsinghmangat.github.io/dynamicalnodes).
|
|
50
50
|
|
|
51
51
|
For a 22 second video describing this framework's intended workflow for, click here: [SAIL 2025 -- Presenting dynamicalnodes](https://youtu.be/5GbVHo6QZrw).
|
|
52
52
|
|
|
53
53
|
---
|
|
54
|
-
## Turtlebot: Whiteboard
|
|
54
|
+
## Turtlebot: Whiteboard ⟶ Python ⟶ ROS 2 ⟶ World
|
|
55
55
|
<table>
|
|
56
56
|
<tr>
|
|
57
57
|
<td align="center">
|
|
58
|
-
<img src="docs/source/_static/
|
|
58
|
+
<img src="docs/source/_static/figures/turtlebot_schematic.svg" alt="TurtleBot Schematic" width="400"/><br/>
|
|
59
59
|
</td>
|
|
60
60
|
<td align="center">
|
|
61
|
-
<img src="docs/source/_static/
|
|
61
|
+
<img src="docs/source/_static/figures/turtlebot_python_simulation.gif" alt="Turtle Software" width="200"/><br/>
|
|
62
62
|
</td>
|
|
63
63
|
<td align="center">
|
|
64
|
-
<img src="docs/source/_static/
|
|
64
|
+
<img src="docs/source/_static/figures/turtlebot_ros_simulation.gif" alt="Turtle Simulation" width="200"/><br/>
|
|
65
65
|
</td>
|
|
66
66
|
<td align="center">
|
|
67
|
-
<img src="docs/source/_static/
|
|
67
|
+
<img src="docs/source/_static/figures/turtlebot_mocab.gif" alt="Turtle Hardware" width="200"/><br/>
|
|
68
68
|
</td>
|
|
69
69
|
</tr>
|
|
70
70
|
</table>
|
|
71
71
|
|
|
72
72
|
<p align="center">
|
|
73
|
-
<img src="docs/source/_static/
|
|
73
|
+
<img src="docs/source/_static/figures/pipeline_theory_python_ros_hardware.png" alt="Step Labels" width="800"/>
|
|
74
74
|
</p>
|
|
75
75
|
|
|
76
76
|
|
dynamicalnodes-0.1.2/README.md
DELETED
|
@@ -1,38 +0,0 @@
|
|
|
1
|
-
# dynamicalnodes: Control Systems on ROS 2
|
|
2
|
-
|
|
3
|
-
> _"Give me a [control system] long enough and a [computer] on which to place it, and I shall move the world."_ - Archimedes
|
|
4
|
-
|
|
5
|
-
**dynamicalnodes** is a Python development framework that bridges the gap between control systems and their implementation onto ROS 2. Designed for hobbyists, students, and academics alike, this framework won't cure cancer, but it can do the next best thing: make robotics easier.
|
|
6
|
-
|
|
7
|
-
To get started, or to explore what this framework has to offer, click here: [dynamicalnodes Documentation](https://nehalsinghmangat.github.io/dynamicalnodes).
|
|
8
|
-
|
|
9
|
-
For a 22 second video describing this framework's intended workflow for, click here: [SAIL 2025 -- Presenting dynamicalnodes](https://youtu.be/5GbVHo6QZrw).
|
|
10
|
-
|
|
11
|
-
---
|
|
12
|
-
## Turtlebot: Whiteboard -> Python -> ROS -> World
|
|
13
|
-
<table>
|
|
14
|
-
<tr>
|
|
15
|
-
<td align="center">
|
|
16
|
-
<img src="docs/source/_static/figure_turtlebot_pipeline/turtlebot.svg" alt="TurtleBot Overview" width="400"/><br/>
|
|
17
|
-
</td>
|
|
18
|
-
<td align="center">
|
|
19
|
-
<img src="docs/source/_static/figure_turtlebot_pipeline/turtlesim_software.gif" alt="Turtle Software" width="200"/><br/>
|
|
20
|
-
</td>
|
|
21
|
-
<td align="center">
|
|
22
|
-
<img src="docs/source/_static/figure_turtlebot_pipeline/turtlesim_simulation.gif" alt="Turtle Simulation" width="200"/><br/>
|
|
23
|
-
</td>
|
|
24
|
-
<td align="center">
|
|
25
|
-
<img src="docs/source/_static/figure_turtlebot_pipeline/turtlesim_hardware.gif" alt="Turtle Hardware" width="200"/><br/>
|
|
26
|
-
</td>
|
|
27
|
-
</tr>
|
|
28
|
-
</table>
|
|
29
|
-
|
|
30
|
-
<p align="center">
|
|
31
|
-
<img src="docs/source/_static/figure_turtlebot_pipeline/labels_for_steps_from_theory_to_hardware.png" alt="Step Labels" width="800"/>
|
|
32
|
-
</p>
|
|
33
|
-
|
|
34
|
-
|
|
35
|
-
|
|
36
|
-
|
|
37
|
-
|
|
38
|
-
|
|
@@ -1,226 +0,0 @@
|
|
|
1
|
-
"""
|
|
2
|
-
Core abstraction for modeling dynamical systems in dynamicalnodes.
|
|
3
|
-
|
|
4
|
-
A dynamical system is defined by two functions:
|
|
5
|
-
- f: State transition function (optional) - computes next state
|
|
6
|
-
- h: Observation function (required) - computes output from state
|
|
7
|
-
|
|
8
|
-
This abstraction allows any control system component (plants, controllers,
|
|
9
|
-
observers, reference signals) to be modeled uniformly and composed together.
|
|
10
|
-
"""
|
|
11
|
-
|
|
12
|
-
import inspect
|
|
13
|
-
from typing import Any, Callable, Dict, Optional
|
|
14
|
-
|
|
15
|
-
|
|
16
|
-
class DynamicalSystem:
|
|
17
|
-
"""
|
|
18
|
-
A discrete-time dynamical system with state transition and observation functions.
|
|
19
|
-
|
|
20
|
-
This class provides a unified abstraction for modeling control system components.
|
|
21
|
-
Each component is defined by:
|
|
22
|
-
|
|
23
|
-
- **f(x_k, u_k, ...) -> x_{k+1}**: State transition function (optional)
|
|
24
|
-
- **h(x_k, u_k, ...) -> y_k**: Observation/output function (required)
|
|
25
|
-
|
|
26
|
-
The `step()` method executes one timestep: it calls f to update state,
|
|
27
|
-
then calls h to compute the output.
|
|
28
|
-
|
|
29
|
-
Parameters
|
|
30
|
-
----------
|
|
31
|
-
f : Callable, optional
|
|
32
|
-
State transition function. Signature: f(x_k, u_k, ...) -> x_{k+1}
|
|
33
|
-
If None, the system is stateless (e.g., a reference signal generator).
|
|
34
|
-
h : Callable
|
|
35
|
-
Observation function. Signature: h(x_k, u_k, ...) -> y_k
|
|
36
|
-
This function computes the system output.
|
|
37
|
-
|
|
38
|
-
Examples
|
|
39
|
-
--------
|
|
40
|
-
Stateless reference signal (step function):
|
|
41
|
-
|
|
42
|
-
>>> def h_ref(tk, params):
|
|
43
|
-
... u0, t_step = params
|
|
44
|
-
... return u0 if tk >= t_step else 0.0
|
|
45
|
-
>>> ref_signal = DynamicalSystem(h=h_ref)
|
|
46
|
-
>>> ref_signal.step(tk=0.0, params=(100, 15)) # Before step time
|
|
47
|
-
0.0
|
|
48
|
-
>>> ref_signal.step(tk=20.0, params=(100, 15)) # After step time
|
|
49
|
-
100
|
|
50
|
-
|
|
51
|
-
Stateful plant (discrete-time integrator):
|
|
52
|
-
|
|
53
|
-
>>> def f_plant(x_k, u_k, dt):
|
|
54
|
-
... return x_k + u_k * dt
|
|
55
|
-
>>> def h_plant(x_k, u_k, dt):
|
|
56
|
-
... return x_k
|
|
57
|
-
>>> plant = DynamicalSystem(f=f_plant, h=h_plant)
|
|
58
|
-
>>> x_next, y = plant.step(x_k=0.0, u_k=1.0, dt=0.1)
|
|
59
|
-
>>> x_next
|
|
60
|
-
0.1
|
|
61
|
-
|
|
62
|
-
Notes
|
|
63
|
-
-----
|
|
64
|
-
The `_smart_call` mechanism allows flexible parameter binding. Functions
|
|
65
|
-
only receive the parameters they declare in their signature, enabling
|
|
66
|
-
components with different interfaces to be composed together.
|
|
67
|
-
|
|
68
|
-
See Also
|
|
69
|
-
--------
|
|
70
|
-
ROSNode : Wraps a DynamicalSystem as a ROS2 node for deployment.
|
|
71
|
-
"""
|
|
72
|
-
|
|
73
|
-
def __init__(
|
|
74
|
-
self,
|
|
75
|
-
*,
|
|
76
|
-
f: Optional[Callable] = None,
|
|
77
|
-
h: Callable,
|
|
78
|
-
) -> None:
|
|
79
|
-
"""
|
|
80
|
-
Initialize a DynamicalSystem with state transition and observation functions.
|
|
81
|
-
|
|
82
|
-
Parameters
|
|
83
|
-
----------
|
|
84
|
-
f : Callable, optional
|
|
85
|
-
State transition function: f(x_k, u_k, ...) -> x_{k+1}
|
|
86
|
-
h : Callable
|
|
87
|
-
Observation function: h(x_k, u_k, ...) -> y_k
|
|
88
|
-
"""
|
|
89
|
-
self._f = f
|
|
90
|
-
self._h = h
|
|
91
|
-
|
|
92
|
-
@property
|
|
93
|
-
def f(self) -> Optional[Callable]:
|
|
94
|
-
"""State transition function f(x_k, u_k, ...) -> x_{k+1}, or None if stateless."""
|
|
95
|
-
return self._f
|
|
96
|
-
|
|
97
|
-
@f.setter
|
|
98
|
-
def f(self, f: Optional[Callable]) -> None:
|
|
99
|
-
self._f = f
|
|
100
|
-
|
|
101
|
-
@property
|
|
102
|
-
def h(self) -> Callable:
|
|
103
|
-
"""Observation function h(x_k, u_k, ...) -> y_k."""
|
|
104
|
-
return self._h
|
|
105
|
-
|
|
106
|
-
@h.setter
|
|
107
|
-
def h(self, h: Callable) -> None:
|
|
108
|
-
self._h = h
|
|
109
|
-
|
|
110
|
-
@staticmethod
|
|
111
|
-
def _smart_call(
|
|
112
|
-
func: Callable[..., Any],
|
|
113
|
-
**kwargs: Any,
|
|
114
|
-
) -> Any:
|
|
115
|
-
"""
|
|
116
|
-
Call a function with smart parameter binding by name.
|
|
117
|
-
|
|
118
|
-
This method inspects the function signature and passes only the
|
|
119
|
-
parameters that the function declares. This enables flexible
|
|
120
|
-
composition of components with different interfaces.
|
|
121
|
-
|
|
122
|
-
Parameters
|
|
123
|
-
----------
|
|
124
|
-
func : Callable
|
|
125
|
-
The function to call.
|
|
126
|
-
**kwargs : Any
|
|
127
|
-
Keyword arguments pool. Only arguments matching the function's
|
|
128
|
-
declared parameters are passed.
|
|
129
|
-
|
|
130
|
-
Returns
|
|
131
|
-
-------
|
|
132
|
-
Any
|
|
133
|
-
The return value of the function.
|
|
134
|
-
|
|
135
|
-
Notes
|
|
136
|
-
-----
|
|
137
|
-
If the function has a **kwargs parameter, all remaining kwargs
|
|
138
|
-
from the pool are passed through.
|
|
139
|
-
|
|
140
|
-
Examples
|
|
141
|
-
--------
|
|
142
|
-
Functions receive only the parameters they declare:
|
|
143
|
-
|
|
144
|
-
>>> def needs_time(tk, gain): return tk * gain
|
|
145
|
-
>>> def needs_state(xk, gain): return (xk, gain)
|
|
146
|
-
>>> def needs_both(tk, xk, gain): return (tk, xk, gain)
|
|
147
|
-
|
|
148
|
-
>>> DynamicalSystem._smart_call(needs_time, tk=1.0, xk=[0, 0], gain=2.0)
|
|
149
|
-
2.0
|
|
150
|
-
>>> DynamicalSystem._smart_call(needs_state, tk=1.0, xk=[5, 5], gain=2.0)
|
|
151
|
-
([5, 5], 2.0)
|
|
152
|
-
>>> DynamicalSystem._smart_call(needs_both, tk=1.0, xk='state', gain=2.0)
|
|
153
|
-
(1.0, 'state', 2.0)
|
|
154
|
-
"""
|
|
155
|
-
sig = inspect.signature(func)
|
|
156
|
-
pool: Dict[str, Any] = dict(kwargs)
|
|
157
|
-
|
|
158
|
-
# Filter: only pass arguments the function declares
|
|
159
|
-
call_kwargs: Dict[str, Any] = {}
|
|
160
|
-
for name, p in sig.parameters.items():
|
|
161
|
-
if p.kind in (
|
|
162
|
-
inspect.Parameter.POSITIONAL_OR_KEYWORD,
|
|
163
|
-
inspect.Parameter.KEYWORD_ONLY,
|
|
164
|
-
):
|
|
165
|
-
if name in pool:
|
|
166
|
-
call_kwargs[name] = pool.pop(name)
|
|
167
|
-
|
|
168
|
-
# Pass remaining kwargs if function has **kwargs
|
|
169
|
-
if any(
|
|
170
|
-
p.kind is inspect.Parameter.VAR_KEYWORD for p in sig.parameters.values()
|
|
171
|
-
):
|
|
172
|
-
call_kwargs.update(pool)
|
|
173
|
-
|
|
174
|
-
return func(**call_kwargs)
|
|
175
|
-
|
|
176
|
-
def step(self, **kwargs: Any) -> Any:
|
|
177
|
-
"""
|
|
178
|
-
Execute one timestep of the dynamical system.
|
|
179
|
-
|
|
180
|
-
This method calls the state transition function f (if defined) and
|
|
181
|
-
the observation function h with the provided keyword arguments.
|
|
182
|
-
|
|
183
|
-
Parameters
|
|
184
|
-
----------
|
|
185
|
-
**kwargs : Any
|
|
186
|
-
Keyword arguments passed to f and h. Each function receives
|
|
187
|
-
only the kwargs it declares in its signature via `_smart_call`.
|
|
188
|
-
|
|
189
|
-
A reserved key ``_state`` may be passed to inject the current
|
|
190
|
-
state without knowing ``f``'s first parameter name. It is
|
|
191
|
-
removed from kwargs before dispatch and inserted under the
|
|
192
|
-
correct name automatically.
|
|
193
|
-
|
|
194
|
-
Returns
|
|
195
|
-
-------
|
|
196
|
-
Any
|
|
197
|
-
If f is None (stateless system):
|
|
198
|
-
Returns y_k = h(...)
|
|
199
|
-
If f is defined (stateful system):
|
|
200
|
-
Returns (x_{k+1}, y_k) = (f(...), h(...))
|
|
201
|
-
|
|
202
|
-
Examples
|
|
203
|
-
--------
|
|
204
|
-
Stateless system (no f):
|
|
205
|
-
|
|
206
|
-
>>> def h(tk): return tk * 2
|
|
207
|
-
>>> sys = DynamicalSystem(h=h)
|
|
208
|
-
>>> sys.step(tk=5.0)
|
|
209
|
-
10.0
|
|
210
|
-
|
|
211
|
-
Stateful system (with f):
|
|
212
|
-
|
|
213
|
-
>>> def f(x, u): return x + u
|
|
214
|
-
>>> def h(x, u): return x
|
|
215
|
-
>>> sys = DynamicalSystem(f=f, h=h)
|
|
216
|
-
>>> x_next, y = sys.step(x=0.0, u=1.0)
|
|
217
|
-
>>> x_next, y
|
|
218
|
-
(1.0, 0.0)
|
|
219
|
-
"""
|
|
220
|
-
if self.f is None:
|
|
221
|
-
return self._smart_call(self.h, **kwargs)
|
|
222
|
-
|
|
223
|
-
return (
|
|
224
|
-
self._smart_call(self.f, **kwargs),
|
|
225
|
-
self._smart_call(self.h, **kwargs),
|
|
226
|
-
)
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
{dynamicalnodes-0.1.2 → dynamicalnodes-0.1.3}/src/dynamicalnodes.egg-info/dependency_links.txt
RENAMED
|
File without changes
|
|
File without changes
|
|
File without changes
|