The goal of the optimize_time flag in the OptimizationBuilder interface was originally meant as a helper for when the user wanted to make dt part of the decision variables. In reality, this doesn't happen very much and in the case that it does, it is not such a big ask for the user to add this manually. Furthermore, because of optimize_time, it convolutes the methods below, making it very difficult to follow the logic (these methods are useful).
|
def _integr(self, m, n): |
|
"""Returns an integration function where m is the state dimension, and n is the number of trajectory points.""" |
|
xd = cs.SX.sym("xd", m) |
|
x0 = cs.SX.sym("x0", m) |
|
x1 = cs.SX.sym("x1", m) |
|
dt = cs.SX.sym("dt") |
|
integr = cs.Function("integr", [x0, x1, xd, dt], [x0 + dt * xd - x1]) |
|
return integr.map(n) |
|
|
|
def integrate_model_states(self, name, time_deriv, dt=None): |
|
"""Integrates the model states over time. |
|
|
|
Syntax |
|
------ |
|
|
|
builder.integrate_model_states(name, time_deriv, dt=None) |
|
|
|
Parameters |
|
---------- |
|
|
|
name (string) |
|
Name of the model. |
|
|
|
time_deriv (int) |
|
The time-deriviative required (i.e. position is 0, velocity is 1, etc.). |
|
|
|
dt (None, float, or array-like: casadi.SX, casadi.DM, or list or numpy.ndarray) |
|
Integration time step. |
|
|
|
""" |
|
|
|
if self.optimize_time and dt is not None: |
|
raise ValueError("dt is given but user specified optimize_time as True") |
|
if not self.optimize_time and dt is None: |
|
raise ValueError("dt is not given") |
|
|
|
model = self.get_model(name) |
|
xd = self.get_model_states(name, time_deriv) |
|
x = self.get_model_states(name, time_deriv - 1) |
|
n = x.shape[1] |
|
if self.derivs_align: |
|
xd = xd[:, :-1] |
|
|
|
if self.optimize_time: |
|
dt = self.get_dt()[:n] |
|
else: |
|
dt = cs.vec(dt) |
|
assert dt.shape[0] in { |
|
1, |
|
n - 1, |
|
}, f"dt should be scalar or have {n-1} elements" |
|
if dt.shape[0] == 1: |
|
dt = dt * cs.DM.ones(n - 1) |
|
dt = cs.vec(dt).T # ensure dt is 1-by-(n-1) array |
|
|
|
if isinstance(model, RobotModel): |
|
integr = self._integr(model.num_opt_joints, n - 1) |
|
else: |
|
integr = self._integr(model.dim, n - 1) |
|
name = f"__integrate_model_states_{name}_{time_deriv}__" |
|
self.add_equality_constraint(name, integr(x[:, :-1], x[:, 1:], xd, dt)) |
@joaomoura24, unless you have any major objections, I will deprecate this feature as part of the #37 PR. I am guessing you probably aren't using this flag in your code?