Strange behavior during forward pass, with custom dynamics

[tomekatat]

Hi everyone,

I am using the proxddp solver with my own defined discrete dynamics, similarly in: https://github.com/Simple-Robotics/aligator/blob/main/tests/python/test_custom_pyfunctions.py
I noticed a behavior in the forward pass, that I do not understand.

Here is my discrete dynamics class:

import aligator
import numpy as np
from aligator import constraints, manifolds, dynamics

class DiscDynamicModel(dynamics.ExplicitDynamicsModel):
    shared_model = None
    shared_data = None
    shared_timestep = None

    def __init__(self):
        self.model = DiscDynamicModel.shared_model
        self.data = DiscDynamicModel.shared_data

        space = manifolds.VectorSpace(self.model.nx)
        self.dt = DiscDynamicModel.shared_timestep

        super().__init__(space, self.model.nu)

    def __getinitargs__(self):
        return ()

    def __deepcopy__(self, memo):
        cls = self.__class__
        new_obj = cls.__new__(cls)
        memo[id(self)] = new_obj

        new_obj.model = self.model
        new_obj.data = self.data
        new_obj.dt = self.dt

        dynamics.ExplicitDynamicsModel.__init__(new_obj, self.space, self.NU)
        return new_obj

    def forward(self, x, u, data: aligator.dynamics.ExplicitDynamicsData):
        x_next = calculate_next_state(self.model, self.data, x, u, self.dt)
        data.xnext[:] = x_next.copy()

    def dForward(self, x, u, data: aligator.dynamics.ExplicitDynamicsData):
        A = calculate_state_jacobian(self.model, self.data, x, u, self.dt)
        B = calculate_control_jacobian(self.model, self.data, x, u, self.dt)

        data.Jx[:,:] = A.copy()
        data.Ju[:,:] = B.copy()

I defined the forward function to calculate the next state from x using control u and dforward function to calculate the jacobians wrt states and controls. When I setup my problem, using my discrete dynamics and run the solver I experience the following: the solver calls N-1 times the forward function (where N is the horizon) and then N-1 times the dforward function. This must be the start of the backward pass in the algorithm and if I understand correctly, these calculations are needed for the Q-function.

After this, the solver calls again N-1 times the forward function. This must be the forward pass of the algorithm. Of course the incoming x and u values are changed since the solver calculated the new control values in the backward pass. Here comes the part that I do not understand. If this is the forward pass then every calculated xnext value has to be the x value in the next forward function call. However for me, the x values are different than the previous xnext values. This was the first strange behavior for me. The second strange behavior is that in the solver's result the xs variable is the sequence of incoming x values in the forward pass and not the sequence of calculated xnext values.

I tested this behavior with the linked test_custom_pyfunctions.py class too. In this case, in the forward pass the incoming x values were equal with the xnext values in the previous stage. Therefore the xs solution were also consistent with the xnext values. However this example is simple, so differences can occur compared to my application.

Can someone help me explain this behavior?

Thank you in forward,
Best regards

[ManifoldFR]

If this is the forward pass then every calculated xnext value has to be the x value in the next forward function call. However for me, the x values are different than the previous xnext values

This is the algorithm, which is a multiple-shooting algorithm. The value of xnext doesn't have to be the value of x at the next timestep's function call, only at convergence (within a given feasibility tolerance). This behaviour depends on the rollout type and the initial inverse penalty parameter mu_init. If you set to a nonlinear rollout with a lower mu_init, the states will be dynamically consistent quicker (at the cost of perhaps higher, um, cost, at algorithm convergence).

The second strange behavior is that in the solver's result the xs variable is the sequence of incoming x values in the forward pass and not the sequence of calculated xnext values.

This is the correct behaviour. Why should it be the xnext values? This would mean the entire solution vector is shifted by a timestep, which is not correct.

[tomekatat]

Thanks, this explained my problem!