From 7c6be9329b251498ed0cbb49a66b5e25ad75d6a8 Mon Sep 17 00:00:00 2001 From: Simeon Nedelchev Date: Mon, 5 Feb 2024 14:10:30 +0300 Subject: [PATCH] restructured integrators, added midpoint (#16) * restructured integrators, added midpoint * modified rollout example * modified the integrators, added doc strings * modified the integrators, added doc strings * removed example 01 * Delete examples/01_intro.ipynb * restorted example 01 from master * added example 01 --- .gitignore | 3 +- darli/backend/base.py | 2 +- darli/backend/casadi.py | 2 +- darli/backend/pinocchio.py | 2 +- darli/modeling/base.py | 4 + darli/modeling/functional/state_space.py | 28 +- darli/modeling/integrators/__init__.py | 1 + darli/modeling/integrators/base.py | 108 +++++++- darli/modeling/integrators/fwd_euler.py | 62 +++-- darli/modeling/integrators/mid_point.py | 54 ++++ darli/modeling/integrators/rk4.py | 85 +++--- darli/modeling/state_space/casadi.py | 1 + darli/modeling/state_space/common.py | 22 +- darli/quaternions/quaternions.py | 12 + examples/04_rollout.ipynb | 318 +++++++++++++++++------ 15 files changed, 541 insertions(+), 163 deletions(-) create mode 100644 darli/modeling/integrators/mid_point.py diff --git a/.gitignore b/.gitignore index 0a74f8c..3ee6480 100644 --- a/.gitignore +++ b/.gitignore @@ -159,4 +159,5 @@ cython_debug/ # and can be added to the global gitignore or merged into this file. For a more nuclear # option (not recommended) you can uncomment the following to ignore the entire idea folder. #.idea/ -*TODO \ No newline at end of file +*TODO +.vscode* \ No newline at end of file diff --git a/darli/backend/base.py b/darli/backend/base.py index db5ebb9..88d1b88 100644 --- a/darli/backend/base.py +++ b/darli/backend/base.py @@ -263,8 +263,8 @@ def cone( @abstractmethod def integrate_configuration( self, - dt: float, q: ArrayLike | None = None, v: ArrayLike | None = None, + dt: float = 1, ) -> ArrayLike: ... diff --git a/darli/backend/casadi.py b/darli/backend/casadi.py index 141b16a..acf776d 100644 --- a/darli/backend/casadi.py +++ b/darli/backend/casadi.py @@ -374,9 +374,9 @@ def cone( def integrate_configuration( self, - dt: float | cs.SX, q: ArrayLike | None = None, v: ArrayLike | None = None, + dt: float | cs.SX = 1.0, ) -> ArrayLike: return self.__kindyn.integrate()( q=q if q is not None else self._q, diff --git a/darli/backend/pinocchio.py b/darli/backend/pinocchio.py index a5a7a53..4f6e0c3 100644 --- a/darli/backend/pinocchio.py +++ b/darli/backend/pinocchio.py @@ -485,9 +485,9 @@ def cone( def integrate_configuration( self, - dt: float, q: ArrayLike | None = None, v: ArrayLike | None = None, + dt: float = 1.0, ) -> ArrayLike: return pin.integrate( self.__model, diff --git a/darli/modeling/base.py b/darli/modeling/base.py index b1a9097..6f2a6cd 100644 --- a/darli/modeling/base.py +++ b/darli/modeling/base.py @@ -359,6 +359,10 @@ def update( class StateSpaceBase(ABC): + @property + def integrator(self): + pass + @property @abstractmethod def model(self) -> ModelBase: diff --git a/darli/modeling/functional/state_space.py b/darli/modeling/functional/state_space.py index 20622e6..2b83412 100644 --- a/darli/modeling/functional/state_space.py +++ b/darli/modeling/functional/state_space.py @@ -1,7 +1,8 @@ from ..state_space import CasadiStateSpace from ..base import ModelBase, StateSpaceBase, ArrayLike import casadi as cs -from ..integrators import Integrator, ForwardEuler +from ..integrators import Integrator +from typing import Type class FunctionalStateSpace(StateSpaceBase): @@ -15,6 +16,18 @@ def __init__( else: self.__space = CasadiStateSpace(model) + def set_integrator(self, integrator_cls: Type[Integrator]): + """ + Set the integrator for the system using the provided Integrator class. + + Args: + integrator_cls (Type[Integrator]): The class (constructor) of the integrator to be used. + + Returns: + The result of setting the integrator in the underlying state space. + """ + return self.__space.set_integrator(integrator_cls) + @classmethod def from_space(cls, space: CasadiStateSpace): return cls(space=space) @@ -88,11 +101,7 @@ def time_variation(self): ) def rollout( - self, - dt: float, - n_steps: int, - control_sampling: float | None = None, - integrator: Integrator = ForwardEuler, + self, dt: float, n_steps: int, control_sampling: float | None = None ) -> cs.Function: if control_sampling is None: control_sampling = dt @@ -117,12 +126,7 @@ def rollout( ], [ self.__space.rollout( - self.__space.state, - container, - dt, - n_steps, - control_sampling, - integrator, + self.__space.state, container, dt, n_steps, control_sampling ) ], [ diff --git a/darli/modeling/integrators/__init__.py b/darli/modeling/integrators/__init__.py index 3f9876d..f226c46 100644 --- a/darli/modeling/integrators/__init__.py +++ b/darli/modeling/integrators/__init__.py @@ -2,3 +2,4 @@ from .base import Integrator from .fwd_euler import ForwardEuler from .rk4 import RK4 +from .mid_point import MidPoint diff --git a/darli/modeling/integrators/base.py b/darli/modeling/integrators/base.py index a9eee43..df2afe3 100644 --- a/darli/modeling/integrators/base.py +++ b/darli/modeling/integrators/base.py @@ -1,17 +1,101 @@ -"""Module for base class of integrators""" +"""Module for base class of integrators.""" -from abc import ABC, abstractstaticmethod -from ..base import StateSpaceBase +from abc import ABC, abstractmethod +from ..base import ModelBase from ...arrays import ArrayLike import casadi as cs +from typing import Callable -class Integrator(ABC): - @abstractstaticmethod - def forward( - state_space: StateSpaceBase, - x0: ArrayLike, - qfrc_u: ArrayLike, - dt: float | cs.SX, - ) -> ArrayLike: - pass +class IntegratorBase(ABC): + """ + Abstract base class for integrators. + This class provides the interface that all derived integrator classes should implement. + """ + + def __init__(self, model: ModelBase): + """ + Initialize the IntegratorBase with a model of the dynamic system. + + Args: + model (ModelBase): The model on which the integration will be performed. + """ + # simplify notation for integration on manifold + + @abstractmethod + def tangent_step(self, x: ArrayLike, tang_x: ArrayLike) -> ArrayLike: + """ + Abstract method to perform step assuming the configuration is on a manifold. + + Args: + x (ArrayLike): The current state vector of the system. + tang_x (ArrayLike): The tangent vector at state x. + + Returns: + ArrayLike: The state vector after performing the tangent step. + """ + + @abstractmethod + def forward(self, x0: ArrayLike, u: ArrayLike, dt: float | cs.SX) -> ArrayLike: + """ + Abstract method to perform a forward integration step. + + Args: + derivative (Callable[[ArrayLike, ArrayLike, ArrayLike], ArrayLike]): + A function that computes the tangent of the state. + x0 (ArrayLike): + The initial state from which to start the integration. + u (ArrayLike): The input forces affecting the system's dynamics. + dt (float | cs.SX): + The timestep over which to integrate. + + Returns: + ArrayLike: The state vector after performing the forward integration. + """ + + def derivative(self, x: ArrayLike, u: ArrayLike) -> ArrayLike: + """Calculate the derivative (tangent) of the state vector for particular model""" + + +class Integrator(IntegratorBase): + def __init__(self, model: ModelBase): + """ + Initialize the Integrator with a dynamic system model. + + Args: + model (ModelBase): The model on which the integration will be performed. + """ + super().__init__(model) + self.__model = model + self.nq = self.__model.nq + self.nv = self.__model.nv + self.__manifold_integrate = self.__model.backend.integrate_configuration + + def tangent_step(self, x: ArrayLike, tang_x: ArrayLike) -> ArrayLike: + """ + Concrete implementation of the tangent step for manifold integration. + + Args: + x (ArrayLike): The current state vector of the system. + tang_x (ArrayLike): The tangent vector at state x. + + Returns: + ArrayLike: The state vector after performing the tangent step. + """ + # Create a container for the new state + container = self.__model.backend.math.zeros(self.nq + self.nv) + # Integrate configuration on the manifold + container[: self.nq] = self.__manifold_integrate( + q=x[: self.nq], v=tang_x[: self.nv] + ) + # Velocity and acceleration are simply updated + container[self.nq :] = x[self.nq :] + tang_x[self.nv :] + + return container.array + + def derivative(self, x: ArrayLike, u: ArrayLike) -> ArrayLike: + q, v = x[: self.nq], x[self.nq :] + container = self.__model.backend.math.zeros(2 * self.nv) + container[: self.nv] = v + container[self.nv :] = self.__model.forward_dynamics(q, v, u) + return container.array diff --git a/darli/modeling/integrators/fwd_euler.py b/darli/modeling/integrators/fwd_euler.py index 3c0d573..db0727e 100644 --- a/darli/modeling/integrators/fwd_euler.py +++ b/darli/modeling/integrators/fwd_euler.py @@ -1,31 +1,53 @@ -"""Module for forward euler integrator""" -from .base import Integrator, StateSpaceBase, ArrayLike, cs +"""Module for forward euler integrator.""" + +from .base import Integrator, ModelBase, ArrayLike, cs +from typing import Callable class ForwardEuler(Integrator): - @staticmethod + """ + Implements the Euler method for numerical integration. + + The Euler method is a simple first-order integration method that updates + the state by taking a single step forward using the derivative of the + state at the current position. This method can be used for systems + evolving on manifolds and is computationally less intensive than higher-order methods. + """ + + def __init__(self, model: ModelBase): + """ + Initialize the Euler integrator with a model that defines system dynamics, + which may include evolution on a manifold. + + Args: + model (ModelBase): The DARLI model providing the system dynamics. + """ + super().__init__(model) + def forward( - state_space: StateSpaceBase, + self, x0: ArrayLike, - qfrc_u: ArrayLike, + u: ArrayLike, dt: cs.SX | float, - ): - nq = state_space.model.nq - nv = state_space.model.nv + ) -> ArrayLike: + """ + Perform a single Euler integration step. - q, v = x0[:nq], x0[nq:] - vdot = state_space.model.forward_dynamics(q, v, qfrc_u) + Args: + derivative: A function that computes the tangent of the state. + x0: Initial state vector, which may include manifold-valued components. + u: Inputs forces acting on the system. + dt: Time step for integration. - # forward euler to integrate velocity - integrated_v = v + dt * vdot + Returns: + The estimated state vector after the Euler integration step. + """ - # pinocchio fancy lie-group integration - integrated_q = state_space.model.backend.integrate_configuration( - dt, q, integrated_v - ) + # Calculate the derivative at the current position + log = self.derivative(x0, u) - container = state_space.model.backend.math.zeros(nq + nv).array - container[:nq] = integrated_q - container[nq:] = integrated_v + # Update the state on manifold by taking a step along the tangent + # and projecting back onto the manifold + exp = self.tangent_step(x0, dt * log) - return container + return exp diff --git a/darli/modeling/integrators/mid_point.py b/darli/modeling/integrators/mid_point.py new file mode 100644 index 0000000..bd6556a --- /dev/null +++ b/darli/modeling/integrators/mid_point.py @@ -0,0 +1,54 @@ +"""Module for forward euler integrator""" +from .base import Integrator, ModelBase, ArrayLike, cs +from typing import Callable + + +class MidPoint(Integrator): + """ + Implements the Midpoint method for numerical integration. + + The Midpoint method is a second-order method that computes the derivative + at the midpoint of the interval to update the state. It provides a better + approximation than the Euler method with modest computational requirements. + This method can handle systems with state spaces that include manifolds. + """ + + def __init__(self, model: ModelBase): + """ + Initialize the Midpoint integrator with a model that defines system dynamics, + which may include manifold evolution. + + Args: + model (ModelBase): The DARLI model providing the system dynamics. + """ + super().__init__(model) + + def forward( + self, + x0: ArrayLike, + u: ArrayLike, + dt: cs.SX | float, + ) -> ArrayLike: + """ + Perform a single Midpoint integration step. + + Args: + derivative: A function that computes the tangent of the state. + x0: Initial state vector, which may include manifold-valued components. + u: Inputs forces acting on the system. + dt: Time step for integration. + + Returns: + The estimated state vector after the Midpoint integration step. + """ + # Compute the first time derivative of the state (Euler step) + k1_log = self.derivative(x0, u) + + # Compute the state at the mid-point using the initial slope (k1_log) + k2_exp = self.tangent_step(x0, 0.5 * dt * k1_log) + + # Compute the time derivative at the mid-point, based on the estimated state (k2_exp) + k2_log = self.derivative(k2_exp, u) + + # Perform the full step using the slope at the mid-point (k2_log) + return self.tangent_step(x0, dt * k2_log) diff --git a/darli/modeling/integrators/rk4.py b/darli/modeling/integrators/rk4.py index 833de19..d95fcc7 100644 --- a/darli/modeling/integrators/rk4.py +++ b/darli/modeling/integrators/rk4.py @@ -1,43 +1,58 @@ """Module for RK4 integrator""" -from .base import Integrator, StateSpaceBase, ArrayLike, cs +from .base import Integrator, ModelBase, ArrayLike, cs +from typing import Callable class RK4(Integrator): - @staticmethod - def forward( - state_space: StateSpaceBase, - x0: ArrayLike, - qfrc_u: ArrayLike, - h: cs.SX | float, - ): - nq = state_space.model.nq - nv = state_space.model.nv - - def tangent_int(x, tang_x): - """tangent integration of state by its increment""" - - # simplify notation for integration on manifold - manifold_int = state_space.model.backend.integrate_configuration - - container = state_space.model.backend.math.zeros(nq + nv) - - # configuration is integrated on manifold using pinocchio implementation - container[:nq] = manifold_int(h, x[:nq], tang_x[nv:]) + """ + Implements the Runge-Kutta 4th order (RK4) method. - # velocity and acceleration are in the same space and do not require specific treatment - container[nq:] = x[nq:] + h * tang_x[nv:] + This implementation of RK4 is capable of handling systems that evolve not just in + Euclidean space but also on manifolds. This is particularly useful for + models that include rotational dynamics, where the state variables (such as + quaternions) evolve on a manifold. + """ - return container.array + def __init__(self, model: ModelBase): + """ + Initialize the RK4 integrator with a model that defines system dynamics, + possibly on a manifold (quaternions in floating base). - k1 = state_space.derivative(x0[:nq], x0[nq:], qfrc_u) + Args: + model (ModelBase): The DARLI model providing the system dynamics. + """ + super().__init__(model) - k2_ = tangent_int(x0, 0.5 * h * k1) - k2 = state_space.derivative(k2_[:nq], k2_[nq:], qfrc_u) - - k3_ = tangent_int(x0, 0.5 * h * k2) - k3 = state_space.derivative(k3_[:nq], k3_[nq:], qfrc_u) - - k4_ = tangent_int(x0, h * k3) - k4 = state_space.derivative(k4_[:nq], k4_[nq:], qfrc_u) - - return tangent_int(x0, (h / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4)) + def forward( + self, + x0: ArrayLike, + u: ArrayLike, + dt: cs.SX | float, + ) -> ArrayLike: + """ + Perform a single RK4 integration step, suitable for state spaces that + might include manifolds (floating base). + + Args: + derivative: A function that computes the tangent of the state. + x0: Initial state vector, which may include manifold-valued components. + u: Inputs forces acting on the system. + dt: Time step for integration. + + Returns: + The estimated state vector after the RK4 integration step. + """ + + # Calculate the four increments from the derivative function + k1_log = self.derivative(x0, u) + k2_exp = self.tangent_step(x0, 0.5 * dt * k1_log) + k2_log = self.derivative(k2_exp, u) + k3_exp = self.tangent_step(x0, 0.5 * dt * k2_log) + k3_log = self.derivative(k3_exp, u) + k4_exp = self.tangent_step(x0, dt * k3_log) + k4_log = self.derivative(k4_exp, u) + + # Combine the four increments for the final state estimate + return self.tangent_step( + x0, (dt / 6.0) * (k1_log + 2 * k2_log + 2 * k3_log + k4_log) + ) diff --git a/darli/modeling/state_space/casadi.py b/darli/modeling/state_space/casadi.py index c73063f..8d701a8 100644 --- a/darli/modeling/state_space/casadi.py +++ b/darli/modeling/state_space/casadi.py @@ -4,6 +4,7 @@ from ...backend import CasadiBackend from ...arrays import CasadiLikeFactory from ...quaternions import state_tangent_map +from ..integrators import Integrator, ForwardEuler class CasadiStateSpace(StateSpace): diff --git a/darli/modeling/state_space/common.py b/darli/modeling/state_space/common.py index 528852b..6ecda7f 100644 --- a/darli/modeling/state_space/common.py +++ b/darli/modeling/state_space/common.py @@ -6,17 +6,28 @@ from ...quaternions import left_mult, expand_map from ..integrators import Integrator, ForwardEuler +# state_space.discretize -> DiscreteStateSpace + +# DiscreteStateSpace(model) class StateSpace(StateSpaceBase): def __init__(self, model: ModelBase) -> None: self.__model: ModelBase = model + self.__integrator: Integrator = ForwardEuler(self.__model) self.__force_jacobians: Dict[str, ArrayLike] = {} + def set_integrator(self, integrator: Integrator): + self.__integrator = integrator(self.__model) + @property def model(self): return self.__model + @property + def integrator(self): + return self.__integrator + @property def force_jacobians(self): return self.__force_jacobians @@ -36,14 +47,14 @@ def derivative( q: ArrayLike | None = None, v: ArrayLike | None = None, u: ArrayLike | None = None, - ): + ) -> ArrayLike: nv = self.__model.nv container = self.model.backend.math.zeros(2 * nv) - container[:nv] = v - container[nv:] = self.model.forward_dynamics(q, v, u) + container[: self.__model.nq] = q + container[self.__model.nq :] = v - return container.array + return self.__integrator.derivative(container.array, u) def time_variation( self, @@ -102,7 +113,6 @@ def rollout( dt: cs.SX | float, n_steps: int, control_sampling: cs.SX | float | None = None, - integrator: Integrator = ForwardEuler, ) -> ArrayLike: """ Rollout function propagates the state forward in time using the input and the state derivative @@ -138,7 +148,7 @@ def rollout( control_time += control_sampling control = controls[:, control_i] - state = integrator.forward(self, state, control, dt) + state = self.__integrator.forward(state, control, dt) time += dt diff --git a/darli/quaternions/quaternions.py b/darli/quaternions/quaternions.py index 3d91ff1..b498a79 100644 --- a/darli/quaternions/quaternions.py +++ b/darli/quaternions/quaternions.py @@ -1,6 +1,18 @@ from ..arrays import ArrayLikeFactory, NumpyLikeFactory +# CBF +# h(q) >= 0 +# ddh + beta*dh + alpha*h >= 0 +# J dv + Jdot@v + beta*J@v + alpha*h >= 0 +# +# Collision +# +# Jdot = (J_now - J_prev)/dt + + +# Make it scalar last + def expand_map(math: ArrayLikeFactory = NumpyLikeFactory): """ Computes the expand map matrix for 3-dimensional vectors. diff --git a/examples/04_rollout.ipynb b/examples/04_rollout.ipynb index 8a438c1..7c79ec1 100644 --- a/examples/04_rollout.ipynb +++ b/examples/04_rollout.ipynb @@ -9,7 +9,7 @@ "from robot_descriptions.a1_description import URDF_PATH\n", "from darli.backend import PinocchioBackend, CasadiBackend, JointType\n", "from darli.modeling import Robot, Functional\n", - "from darli.modeling.integrators import ForwardEuler, RK4\n", + "from darli.modeling.integrators import ForwardEuler, RK4, MidPoint\n", "import numpy as np" ] }, @@ -18,6 +18,18 @@ "execution_count": 2, "metadata": {}, "outputs": [], + "source": [ + "dt = 1e-3\n", + "n_steps = 20\n", + "control_sampling = 5e-3\n", + "samples = int(dt * n_steps / control_sampling)" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], "source": [ "model = Functional(CasadiBackend(URDF_PATH, root_joint=JointType.FREE_FLYER))\n", "model.update_selector(passive_joints=range(6))" @@ -25,7 +37,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 4, "metadata": {}, "outputs": [ { @@ -36,7 +48,7 @@ " 0.14404357, 1.45427351])" ] }, - "execution_count": 3, + "execution_count": 4, "metadata": {}, "output_type": "execute_result" } @@ -50,7 +62,7 @@ }, { "cell_type": "code", - "execution_count": 4, + "execution_count": 5, "metadata": {}, "outputs": [ { @@ -59,7 +71,7 @@ "((37,), 19, 18)" ] }, - "execution_count": 4, + "execution_count": 5, "metadata": {}, "output_type": "execute_result" } @@ -82,23 +94,23 @@ }, { "cell_type": "code", - "execution_count": 5, + "execution_count": 6, "metadata": {}, "outputs": [ { "data": { "text/plain": [ - "Function(rollout:(state[37],controls[12x15])->(next_state[37]) SXFunction)" + "Function(rollout:(state[37],controls[12x4])->(next_state[37]) SXFunction)" ] }, - "execution_count": 5, + "execution_count": 6, "metadata": {}, "output_type": "execute_result" } ], "source": [ "euler_roll = model.state_space.rollout(\n", - " dt=2e-3, n_steps=30, control_sampling=4e-3, integrator=ForwardEuler\n", + " dt=dt, n_steps=n_steps, control_sampling=control_sampling\n", ")\n", "\n", "euler_roll" @@ -106,14 +118,34 @@ }, { "cell_type": "code", - "execution_count": 6, + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "DM([6.86254e-05, -0.000313681, -0.00182676, -0.017466, -0.000772986, -9.55388e-05, 0.999847, 0.0469618, -0.0236709, 0.0713617, 0.0511848, 0.0695515, -0.148044, 0.04396, 0.0031811, -0.00541951, 0.0402101, -0.0454539, 0.123088, 0.00676575, -0.0263639, -0.197224, -3.62433, -0.152288, -0.0232693, 4.89491, -2.50181, 7.51287, 5.36967, 7.25888, -15.4562, 4.5822, 0.325292, -0.570832, 4.19973, -4.77632, 12.9125])" + ] + }, + "execution_count": 7, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "euler_roll(x0, tau)" + ] + }, + { + "cell_type": "code", + "execution_count": 8, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ - "1.45 ms ± 193 µs per loop (mean ± std. dev. of 7 runs, 100 loops each)\n" + "302 µs ± 5.64 µs per loop (mean ± std. dev. of 7 runs, 1,000 loops each)\n" ] } ], @@ -123,23 +155,88 @@ }, { "cell_type": "code", - "execution_count": 7, + "execution_count": 9, "metadata": {}, "outputs": [ { "data": { "text/plain": [ - "Function(rollout:(state[37],controls[12x15])->(next_state[37]) SXFunction)" + "Function(rollout:(state[37],controls[12x4])->(next_state[37]) SXFunction)" ] }, - "execution_count": 7, + "execution_count": 9, "metadata": {}, "output_type": "execute_result" } ], "source": [ + "model.state_space.set_integrator(MidPoint)\n", + "\n", + "midpoint_roll = model.state_space.rollout(\n", + " dt=dt, n_steps=n_steps, control_sampling=control_sampling\n", + ")\n", + "\n", + "midpoint_roll" + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "DM([7.17749e-05, -0.000326519, -0.00193021, -0.01834, -0.00080474, -0.000103313, 0.999831, 0.0493509, -0.0249361, 0.075124, 0.0538461, 0.0730974, -0.1556, 0.0461969, 0.00333232, -0.00570596, 0.04227, -0.0478277, 0.129481, 0.00670047, -0.0257048, -0.198048, -3.61433, -0.15033, -0.0238173, 4.8858, -2.50349, 7.5126, 5.36669, 7.24167, -15.4187, 4.57372, 0.323499, -0.570947, 4.19375, -4.77132, 12.8964])" + ] + }, + "execution_count": 10, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "midpoint_roll(x0, tau)" + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "592 µs ± 14.1 µs per loop (mean ± std. dev. of 7 runs, 1,000 loops each)\n" + ] + } + ], + "source": [ + "%timeit midpoint_roll()" + ] + }, + { + "cell_type": "code", + "execution_count": 12, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "Function(rollout:(state[37],controls[12x4])->(next_state[37]) SXFunction)" + ] + }, + "execution_count": 12, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "model.state_space.set_integrator(RK4)\n", + "\n", "rk4_roll = model.state_space.rollout(\n", - " dt=2e-3, n_steps=30, control_sampling=4e-3, integrator=RK4\n", + " dt=dt, n_steps=n_steps, control_sampling=control_sampling\n", ")\n", "\n", "rk4_roll" @@ -147,14 +244,14 @@ }, { "cell_type": "code", - "execution_count": 8, + "execution_count": 13, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ - "5.55 ms ± 151 µs per loop (mean ± std. dev. of 7 runs, 100 loops each)\n" + "1.19 ms ± 4.32 µs per loop (mean ± std. dev. of 7 runs, 1,000 loops each)\n" ] } ], @@ -171,7 +268,7 @@ }, { "cell_type": "code", - "execution_count": 9, + "execution_count": 14, "metadata": {}, "outputs": [], "source": [ @@ -181,89 +278,86 @@ }, { "cell_type": "code", - "execution_count": 10, + "execution_count": 15, + "metadata": {}, + "outputs": [], + "source": [ + "q0 = np.array([0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])\n", + "v0 = np.zeros(model.nv)\n", + "\n", + "x0 = np.concatenate([q0, v0])\n", + "\n", + "np.random.seed(0)\n", + "tau = np.random.randn(model.nu, samples)" + ] + }, + { + "cell_type": "code", + "execution_count": 16, "metadata": {}, "outputs": [ { "data": { "text/plain": [ - "array([ 3.76576015e-04, -5.90776756e-04, -1.86484692e-02, -3.55866284e-02,\n", - " 2.98828478e-04, -2.06836938e-03, 9.99364410e-01, 1.20123327e-01,\n", - " 8.94504016e-02, -2.04843323e-01, 5.97518327e-02, -2.64078009e-02,\n", - " -1.08357083e-02, 9.61331223e-02, 9.85161233e-02, -1.30548863e-01,\n", - " 8.77269278e-02, -3.11990450e-02, 7.02278288e-02, 1.64054750e-02,\n", - " 2.85389016e-02, -6.01619857e-01, -1.77246306e+00, -3.42885311e-02,\n", - " -1.46697075e-01, 2.98211623e+00, 3.88621024e+00, -8.52350431e+00,\n", - " 1.48212236e+00, -2.21680733e+00, 4.25183734e+00, 2.29552203e+00,\n", - " 3.49743072e+00, -5.23065688e+00, 2.12015156e+00, 1.95744729e-01,\n", - " -6.94286048e-01])" + "" ] }, - "execution_count": 10, + "execution_count": 16, "metadata": {}, "output_type": "execute_result" } ], "source": [ - "q0 = np.array([0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])\n", - "v0 = np.zeros(model.nv)\n", - "\n", - "x0 = np.concatenate([q0, v0])\n", - "\n", - "np.random.seed(0)\n", - "tau = np.random.randn(model.nu, 15)\n", - "\n", - "\n", - "model.state_space.rollout(x0, tau, dt=2e-3, control_sampling=4e-3, n_steps=30)" + "model.state_space.integrator" ] }, { "cell_type": "code", - "execution_count": 11, + "execution_count": 17, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ - "1.49 ms ± 180 µs per loop (mean ± std. dev. of 7 runs, 100 loops each)\n" + "187 µs ± 2.84 µs per loop (mean ± std. dev. of 7 runs, 10,000 loops each)\n" ] } ], "source": [ - "%timeit model.state_space.rollout(x0, tau, dt=2e-3, control_sampling=4e-3, n_steps=30, integrator=ForwardEuler)" + "%timeit model.state_space.rollout(x0, tau, dt=dt, n_steps=n_steps, control_sampling=control_sampling)" ] }, { "cell_type": "code", - "execution_count": 12, + "execution_count": 18, "metadata": {}, "outputs": [ { "data": { "text/plain": [ - "(array([ 3.76576015e-04, -5.90776756e-04, -1.86484692e-02, -3.55866284e-02,\n", - " 2.98828478e-04, -2.06836938e-03, 9.99364410e-01, 1.20123327e-01,\n", - " 8.94504016e-02, -2.04843323e-01, 5.97518327e-02, -2.64078009e-02,\n", - " -1.08357083e-02, 9.61331223e-02, 9.85161233e-02, -1.30548863e-01,\n", - " 8.77269278e-02, -3.11990450e-02, 7.02278288e-02, 1.64054750e-02,\n", - " 2.85389016e-02, -6.01619857e-01, -1.77246306e+00, -3.42885311e-02,\n", - " -1.46697075e-01, 2.98211623e+00, 3.88621024e+00, -8.52350431e+00,\n", - " 1.48212236e+00, -2.21680733e+00, 4.25183734e+00, 2.29552203e+00,\n", - " 3.49743072e+00, -5.23065688e+00, 2.12015156e+00, 1.95744729e-01,\n", - " -6.94286048e-01]),\n", + "(array([ 3.04277252e-04, -2.01330463e-04, -1.90248413e-03, -1.17040266e-02,\n", + " -5.15787389e-04, -3.94341397e-04, 9.99931295e-01, 3.22966013e-02,\n", + " 1.50246560e-02, -1.98648484e-02, 2.99230958e-02, 5.13920733e-02,\n", + " -1.17458362e-01, 2.93587807e-02, 5.98572029e-02, -1.30787434e-01,\n", + " 2.92957256e-02, 2.78096767e-04, -1.22598234e-02, 1.79929644e-02,\n", + " -1.11581574e-02, -2.01115830e-01, -1.86671007e+00, -7.24765145e-02,\n", + " -8.40182063e-02, 2.81446975e+00, -2.83148081e-01, 1.99502364e+00,\n", + " 2.41199628e+00, 2.77334598e+00, -6.48657322e+00, 2.17138080e+00,\n", + " 4.52850880e+00, -9.88360657e+00, 2.22627337e+00, 3.21107178e-01,\n", + " -1.38804941e+00]),\n", " 'quaternion norm',\n", - " 1.0)" + " 0.9999999999999999)" ] }, - "execution_count": 12, + "execution_count": 18, "metadata": {}, "output_type": "execute_result" } ], "source": [ "euler_state = model.state_space.rollout(\n", - " x0, tau, dt=2e-3, control_sampling=4e-3, n_steps=30, integrator=ForwardEuler\n", + " x0, tau, dt=dt, n_steps=n_steps, control_sampling=control_sampling\n", ")\n", "\n", "euler_state, \"quaternion norm\", np.linalg.norm(euler_state[3:7])" @@ -271,51 +365,127 @@ }, { "cell_type": "code", - "execution_count": 13, + "execution_count": 19, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ - "8.31 ms ± 972 µs per loop (mean ± std. dev. of 7 runs, 100 loops each)\n" + "368 µs ± 1.34 µs per loop (mean ± std. dev. of 7 runs, 1,000 loops each)\n" ] } ], "source": [ - "%timeit model.state_space.rollout(x0, tau, dt=2e-3, control_sampling=4e-3, n_steps=30, integrator=RK4)" + "model.state_space.set_integrator(MidPoint)\n", + "\n", + "%timeit model.state_space.rollout(x0, tau, dt=dt, n_steps=n_steps, control_sampling=control_sampling)" ] }, { "cell_type": "code", - "execution_count": 14, + "execution_count": 20, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "(array([ 3.13056814e-04, -2.06007071e-04, -2.00579933e-03, -1.21762022e-02,\n", + " -5.35876488e-04, -4.17895753e-04, 9.99925636e-01, 3.37227998e-02,\n", + " 1.48924239e-02, -1.88716407e-02, 3.11635841e-02, 5.27516271e-02,\n", + " -1.20628686e-01, 3.04188482e-02, 6.21083248e-02, -1.35677435e-01,\n", + " 3.04083768e-02, 4.41604814e-04, -1.29512213e-02, 1.79570888e-02,\n", + " -1.07038872e-02, -2.01332224e-01, -1.86844846e+00, -7.30573129e-02,\n", + " -8.43885313e-02, 2.81685816e+00, -2.82157805e-01, 1.99486806e+00,\n", + " 2.41484678e+00, 2.77127195e+00, -6.48019797e+00, 2.17016381e+00,\n", + " 4.52686951e+00, -9.87708295e+00, 2.22696589e+00, 3.21654162e-01,\n", + " -1.38766016e+00]),\n", + " 'quaternion norm',\n", + " 1.0)" + ] + }, + "execution_count": 20, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "midpoint_state = model.state_space.rollout(\n", + " x0, tau, dt=dt, n_steps=n_steps, control_sampling=control_sampling\n", + ")\n", + "\n", + "midpoint_state, \"quaternion norm\", np.linalg.norm(midpoint_state[3:7])" + ] + }, + { + "cell_type": "code", + "execution_count": 21, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "768 µs ± 14.5 µs per loop (mean ± std. dev. of 7 runs, 1,000 loops each)\n" + ] + } + ], + "source": [ + "model.state_space.set_integrator(RK4)\n", + "\n", + "%timeit model.state_space.rollout(x0, tau, dt=dt, n_steps=n_steps, control_sampling=control_sampling)" + ] + }, + { + "cell_type": "code", + "execution_count": 22, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "" + ] + }, + "execution_count": 22, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "model.state_space.model" + ] + }, + { + "cell_type": "code", + "execution_count": 23, "metadata": {}, "outputs": [ { "data": { "text/plain": [ - "(array([ 3.28031153e-05, -2.75608771e-05, -1.19513565e-03, -1.69214588e-03,\n", - " -5.40001291e-05, -1.48067584e-04, 9.99998556e-01, 5.87030990e-03,\n", - " 7.94249395e-03, -1.74507362e-02, 2.84304581e-03, -4.39651886e-03,\n", - " 8.46712564e-03, 4.47790182e-03, 7.01606052e-03, -1.05225893e-02,\n", - " 4.11139492e-03, 4.06005873e-04, -1.39273193e-03, 3.28216779e-05,\n", - " -2.48331438e-05, -1.19517838e-03, -3.38427802e-03, -1.07776273e-04,\n", - " -2.96480264e-04, 5.87030990e-03, 7.94249395e-03, -1.74507362e-02,\n", - " 2.84304581e-03, -4.39651886e-03, 8.46712564e-03, 4.47790182e-03,\n", - " 7.01606052e-03, -1.05225893e-02, 4.11139492e-03, 4.06005873e-04,\n", - " -1.39273193e-03]),\n", + "(array([ 3.13037969e-04, -2.05839271e-04, -2.00601165e-03, -1.21761337e-02,\n", + " -5.35736848e-04, -4.18077826e-04, 9.99925637e-01, 3.37230957e-02,\n", + " 1.48924051e-02, -1.88717933e-02, 3.11656303e-02, 5.27487210e-02,\n", + " -1.20622606e-01, 3.04167959e-02, 6.21072024e-02, -1.35674861e-01,\n", + " 3.04075968e-02, 4.41164010e-04, -1.29509577e-02, 1.79563270e-02,\n", + " -1.06986147e-02, -2.01343010e-01, -1.86843671e+00, -7.30281345e-02,\n", + " -8.44036179e-02, 2.81684357e+00, -2.82184416e-01, 1.99487844e+00,\n", + " 2.41494885e+00, 2.77110956e+00, -6.47989044e+00, 2.17006990e+00,\n", + " 4.52679842e+00, -9.87696459e+00, 2.22693353e+00, 3.21613763e-01,\n", + " -1.38764357e+00]),\n", " 'quaternion norm',\n", " 1.0)" ] }, - "execution_count": 14, + "execution_count": 23, "metadata": {}, "output_type": "execute_result" } ], "source": [ "rk4_state = model.state_space.rollout(\n", - " x0, tau, dt=2e-3, control_sampling=4e-3, n_steps=30, integrator=RK4\n", + " x0, tau, dt=dt, n_steps=n_steps, control_sampling=control_sampling\n", ")\n", "\n", "rk4_state, \"quaternion norm\", np.linalg.norm(rk4_state[3:7])" @@ -338,7 +508,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.10.12" + "version": "3.10.13" } }, "nbformat": 4,