@@ -191,6 +191,17 @@ def test_flat_cost_constr(self):
191
191
assert np .all (x_const [i ] >= lb [i ] * 1.02 )
192
192
assert np .all (x_const [i ] <= ub [i ] * 1.02 )
193
193
194
+ # Solve the same problem with a nonlinear constraint type
195
+ nl_constraints = [
196
+ (sp .optimize .NonlinearConstraint , lambda x , u : x , lb , ub )]
197
+ traj_nlconst = fs .point_to_point (
198
+ flat_sys , timepts , x0 , u0 , xf , uf , cost = cost_fcn ,
199
+ constraints = nl_constraints , basis = fs .PolyFamily (8 ),
200
+ )
201
+ x_nlconst , u_nlconst = traj_nlconst .eval (T )
202
+ np .testing .assert_almost_equal (x_const , x_nlconst )
203
+ np .testing .assert_almost_equal (u_const , u_nlconst )
204
+
194
205
def test_bezier_basis (self ):
195
206
bezier = fs .BezierFamily (4 )
196
207
time = np .linspace (0 , 1 , 100 )
@@ -245,6 +256,26 @@ def test_point_to_point_errors(self):
245
256
cost_fcn = opt .quadratic_cost (
246
257
flat_sys , np .diag ([1 , 1 ]), 1 , x0 = xf , u0 = uf )
247
258
259
+ # Solving without basis specified should be OK
260
+ traj = fs .point_to_point (flat_sys , timepts , x0 , u0 , xf , uf )
261
+ x , u = traj .eval (timepts )
262
+ np .testing .assert_array_almost_equal (x0 , x [:, 0 ])
263
+ np .testing .assert_array_almost_equal (u0 , u [:, 0 ])
264
+ np .testing .assert_array_almost_equal (xf , x [:, - 1 ])
265
+ np .testing .assert_array_almost_equal (uf , u [:, - 1 ])
266
+
267
+ # Adding a cost function generates a warning
268
+ with pytest .warns (UserWarning , match = "optimization not possible" ):
269
+ traj = fs .point_to_point (
270
+ flat_sys , timepts , x0 , u0 , xf , uf , cost = cost_fcn )
271
+
272
+ # Make sure we still solved the problem
273
+ x , u = traj .eval (timepts )
274
+ np .testing .assert_array_almost_equal (x0 , x [:, 0 ])
275
+ np .testing .assert_array_almost_equal (u0 , u [:, 0 ])
276
+ np .testing .assert_array_almost_equal (xf , x [:, - 1 ])
277
+ np .testing .assert_array_almost_equal (uf , u [:, - 1 ])
278
+
248
279
# Try to optimize with insufficient degrees of freedom
249
280
with pytest .warns (UserWarning , match = "optimization not possible" ):
250
281
traj = fs .point_to_point (
@@ -267,3 +298,36 @@ def test_point_to_point_errors(self):
267
298
traj = fs .point_to_point (flat_sys , timepts , x0 , u0 , np .zeros (3 ), uf )
268
299
with pytest .raises (ValueError , match = "Final input: Wrong shape" ):
269
300
traj = fs .point_to_point (flat_sys , timepts , x0 , u0 , xf , np .zeros (3 ))
301
+
302
+ # Different ways of describing constraints
303
+ constraint = opt .input_range_constraint (flat_sys , - 100 , 100 )
304
+
305
+ with pytest .warns (UserWarning , match = "optimization not possible" ):
306
+ traj = fs .point_to_point (
307
+ flat_sys , timepts , x0 , u0 , xf , uf , constraints = constraint ,
308
+ basis = fs .PolyFamily (6 ))
309
+
310
+ x , u = traj .eval (timepts )
311
+ np .testing .assert_array_almost_equal (x0 , x [:, 0 ])
312
+ np .testing .assert_array_almost_equal (u0 , u [:, 0 ])
313
+ np .testing .assert_array_almost_equal (xf , x [:, - 1 ])
314
+ np .testing .assert_array_almost_equal (uf , u [:, - 1 ])
315
+
316
+ # Constraint that isn't a constraint
317
+ with pytest .raises (TypeError , match = "must be a list" ):
318
+ traj = fs .point_to_point (
319
+ flat_sys , timepts , x0 , u0 , xf , uf , constraints = np .eye (2 ),
320
+ basis = fs .PolyFamily (8 ))
321
+
322
+ # Unknown constraint type
323
+ with pytest .raises (TypeError , match = "unknown constraint type" ):
324
+ traj = fs .point_to_point (
325
+ flat_sys , timepts , x0 , u0 , xf , uf ,
326
+ constraints = [(None , 0 , 0 , 0 )], basis = fs .PolyFamily (8 ))
327
+
328
+ # Unsolvable optimization
329
+ constraint = [opt .input_range_constraint (flat_sys , - 0.01 , 0.01 )]
330
+ with pytest .raises (RuntimeError , match = "Unable to solve optimal" ):
331
+ traj = fs .point_to_point (
332
+ flat_sys , timepts , x0 , u0 , xf , uf , constraints = constraint ,
333
+ basis = fs .PolyFamily (8 ))
0 commit comments