def test_constraint_params(acceleration_pc_data):
""" Test constraint satisfaction with cvxpy.
"""
data, constraint = acceleration_pc_data
path, ss, alim = data
# An user of the class
a, b, c, F, g, ubound, xbound = constraint.compute_constraint_params(
path, ss, 1.0)
assert xbound is None
N = ss.shape[0] - 1
dof = path.dof
ps = path.evald(ss)
pss = path.evaldd(ss)
F_actual = np.vstack((np.eye(dof), -np.eye(dof)))
g_actual = np.hstack((alim[:, 1], -alim[:, 0]))
npt.assert_allclose(F, F_actual)
npt.assert_allclose(g, g_actual)
for i in range(0, N + 1):
npt.assert_allclose(a[i], ps[i])
npt.assert_allclose(b[i], pss[i])
npt.assert_allclose(c[i], np.zeros_like(ps[i]))
assert ubound is None
assert xbound is None
def test_jnt_vel_varying_basic():
# constraint
ss_wpts = np.r_[0, 1, 2]
vlim_wpts = [[[-1, 2], [-1, 2]], [[-2, 3], [-2, 3]], [[-1, 0], [-1, 0]]]
vlim_spl = CubicSpline(ss_wpts, vlim_wpts)
constraint = ta.constraint.JointVelocityConstraintVarying(vlim_spl)
# path
coeff = [[1., 2, 3], [-2., -3., 4., 5.]]
path = ta.PolynomialPath(coeff)
gridpoints = np.linspace(0, 2, 10)
_, _, _, _, _, _, xlimit = constraint.compute_constraint_params(
path, gridpoints, 1.0)
# constraint splines
qs = path(gridpoints, 1)
# test
sd = cvx.Variable()
for i in range(ss_wpts.shape[0]):
vlim = vlim_spl(gridpoints[i])
# 2. compute max sd from the data
constraints = [
qs[i] * sd <= vlim[:, 1], qs[i] * sd >= vlim[:, 0], sd >= 0,
sd <= JVEL_MAXSD
]
try:
prob = cvx.Problem(cvx.Maximize(sd), constraints)
prob.solve(solver=cvx.ECOS, abstol=1e-9)
xmax = sd.value**2
prob = cvx.Problem(cvx.Minimize(sd), constraints)
prob.solve(solver=cvx.ECOS, abstol=1e-9)
xmin = sd.value**2
except cvx.SolverError:
continue # ECOS can't solve this problem.
# 3. they should agree
npt.assert_allclose([xmin, xmax], xlimit[i], atol=SMALL)
# assert non-negativity
assert xlimit[i, 0] >= 0