Nonlinear (non-polynomial) cost function with DirectCollocation in Drake

Question

I am trying to formulate a trajectory optimization problem for a glider, where I want to maximize the average horisontal velocity. I have formulated the system as a drakesystem, and the state vector consists of the position and velocity.

Currently, I have something like the following:

dircol = DirectCollocation(
    plant,
    context,
    num_time_samples=N,
    minimum_timestep=min_dt,
    maximum_timestep=max_dt,
)

... # other constraints etc

horisontal_pos = dircol.state()[0:2] # Only (x,y)
time = dircol.time()
dircol.AddFinalCost(-w.T.dot(horisontal_pos) / time)

where AddFinalCost() should replace all instances of state() and time() with the final values, as far as I understand from the documentation. min_dt is non-zero and w is a vector of linear weights.

However, I am getting the following error message

Expression (...) is not a polynomial. ParseCost does not support non-polynomial expression.

which makes me think that there is no way of adding the type of cost function that I am looking for. Is there anything that I am missing?

Thank you in advance!

score 1 · Accepted Answer · answered Nov 21 '20 at 06:07

1

When calling AddFinalCost(e) with e being a symbolic expression, we can only handle it when e is a polynomial function of the state (more precisely, either a quadratic function or a linear function). Hence the error you see complaining that the cost is not polynomial.

You could add the cost like this

def average_speed(v):
    x = v[0]
    time_steps = v[1:]
    return v[0] / np.sum(time_steps)
    
h_vars = [dircol.timestep[i] for i in range(N-1)]
dircol.AddCost(average_speed, vars=[dircol.state(N-1)[0]] + h_vars)

which uses a function average_speed to evaluate the average speed. You could find example of doing this in https://github.com/RobotLocomotion/drake/blob/e5f3c3e5f7927ef675066d97d3afac55d3481305/bindings/pydrake/solvers/test/mathematicalprogram_test.py#L590

answered Nov 21 '20 at 06:07

Hongkai Dai

2,546
11
12

That is exactly what I needed. Thanks a lot! – bernhardpg Nov 21 '20 at 12:27
Follow-up: is it still the case that Drake can't handle non-polynomial cost functions? In particular, I'm hoping to put a `min` or `exp` function as part of the cost – njk23 Nov 30 '21 at 23:41
@njk23, you can add non-polynomial cost, but just not a symbolic form. See the example above where we add a cost using a function `average_speed`. – Hongkai Dai Dec 01 '21 at 02:26

score 0 · Answer 2 · answered Nov 19 '20 at 17:58

0

First, the cost function should be a scalar, but you a vector-valued horisontal_pos / time, which has two entries containing both position_x / dt and position_y / dt, namely a vector as the cost. You should instead provide a scalar valued cost.

Second, it is unclear to me why you divide time in the final cost. As far as I understand it, you want the final position to be close to the origin, so something like position_x² + position_y². The code can look like

dircol.AddFinalCost(horisontal_pos[0]**2 + horisontal_pos[1]**2)

answered Nov 19 '20 at 17:58

Hongkai Dai

2,546
11
12

Thank you for the quick response! I am sorry, I had a typo in the pseudocode, the cost function is in fact scalar (it is the vector product between a weight vector and hor_pos), I have updated it now. The goal of the optimization is to find the trajectory that maximizes the average velocity over the flight path. Maybe I am misunderstanding you, but can't quite see how your cost function gives will maximize the average velocity? – bernhardpg Nov 20 '20 at 11:00

Nonlinear (non-polynomial) cost function with DirectCollocation in Drake

2 Answers2