def cone_constraint(c, x_obs, y_obs,theta_obs, v_obs, r_cone, uk, x, y, theta, obs_dist,ego_dist,xkm1,ykm1,xref,yref):
# -------distance const
# c += cs.fmax(0.0, 1000*(r_cone -cs.sqrt((x_obs-x)**2 +(y_obs-y)**2)))
# ------- cone with activation and deactivation constraints
v_obs_x=cs.cos(theta_obs)*v_obs
v_obs_y=cs.sin(theta_obs)*v_obs
r_vec=cs.vertcat(x-x_obs,y-y_obs)
vab=cs.vertcat(cs.cos(theta)*uk[0]-v_obs_x,cs.sin(theta)*uk[0]-v_obs_y)
rterm = (cs.norm_2(r_vec))**2
lterm = (cs.norm_2(vab))**2
uterm = (cs.dot(r_vec,vab))**2
cone = (r_cone**2)*lterm-(rterm*lterm-uterm)
passed_obs=cs.if_else(obs_dist>ego_dist ,True,False)
obs_faster_than_ego=cs.if_else(uk[0]<cs.norm_2(v_obs) ,True,False)
obs_driving_towards=cs.if_else(passed_obs,False,cs.if_else(v_obs<=0,True,False))
skip_due_to_dir_and_vel=cs.if_else(obs_driving_towards,False,cs.if_else(obs_faster_than_ego,True,False))
skip_due_far_away=cs.if_else(cs.sqrt((x_obs-x)**2 - (y_obs-y)**2)<10,False,True)
skip=cs.logic_or(skip_due_to_dir_and_vel,skip_due_far_away)
deactivate_cone=cs.if_else(passed_obs,True,skip)
c += cs.if_else(deactivate_cone,0,cs.fmax(0.0, cone))
# decide what side to drive
#side_obs =cs.sign((x_obs-xkm1)*(yref-ykm1)-(y_obs-ykm1)*(xref-xkm1)) # neg if on left
# s=cs.sign((x-xkm1)*(y_obs-ykm1)-(y-ykm1)*(x_obs-xkm1)) # neg if on left
# c +=cs.if_else(deactivate_cone,cs.fmax(0.0,s*decide_side*10),0)
return c,False
def cone_constraint(c, x_obs, y_obs, theta_obs, v_obs, r_cone, v, x, y, theta,
passed_obs):
v_obs_x = cs.cos(theta_obs) * v_obs
v_obs_y = cs.sin(theta_obs) * v_obs
r_vec = cs.vertcat(x - x_obs, y - y_obs)
vab = cs.vertcat(cs.cos(theta) * v - v_obs_x, cs.sin(theta) * v - v_obs_y)
rterm = (cs.norm_2(r_vec))**2
lterm = (cs.norm_2(vab))**2
uterm = (cs.dot(r_vec, vab))**2
cone = (r_cone**2) * lterm - (rterm * lterm - uterm)
# c += cs.if_else(passed_obs,0,cs.fmax(0.0, cone))
c += cs.fmax(0.0, cone)
return c
def test_integration_1(self):
u = cs.SX.sym('u', 3)
xi = cs.SX.sym('xi', 2)
p = cs.SX.sym('p', 2)
f = cs.sin(cs.norm_2(p)) * cs.dot(xi, p) * cs.norm_2(u)
fun = cs.Function('f', [u, xi, p], [f])
transpiler = cc.CasadiRustTranspiler(fun, 'xcst_kangaroo')
transpiler.transpile()
self.assertTrue(transpiler.compile())
(u, xi, p) = ([1, 2, 3], [4, 5], [6, 7])
expected = fun(u, xi, p)
result = transpiler.call_rust(u, xi, p)
self.assertAlmostEqual(expected, result[0], 8)
def cone_constraint(c, x_obs, y_obs, theta_obs, v_obs, r_obs, uk, x, y, theta,
obs_dist, ego_dist, xkm1, ykm1, xref, yref):
v_obs_x = cs.cos(theta_obs) * v_obs
v_obs_y = cs.sin(theta_obs) * v_obs
rterm = (x - x_obs)**2 + (y - y_obs)**2
uterm = ((cs.cos(theta) * uk[0] - v_obs_x) * (x - x_obs) +
(cs.sin(theta) * uk[0] - v_obs_y) * (y - y_obs))
lterm = (cs.cos(theta) * uk[0] - v_obs_x)**2 + (cs.sin(theta) * uk[0] -
v_obs_y)**2
# -------distance const
# c += cs.fmax(0.0, r_obs - (x_obs-x)**2 - (y_obs-y)**2)
# -------regular cone
#c += cs.fmax(0.0, r_obs**2*lterm-(rterm*lterm-uterm**2))
# -------cone only when dist ego > dist obs
cone = r_obs**2 * lterm - (rterm * lterm - uterm**2)
passed_obs = cs.if_else((obs_dist - ego_dist) > 0, True, False)
obs_faster_than_ego = cs.if_else(uk[0] < v_obs, True, False)
obs_driving_towards = cs.if_else(
cs.norm_2(theta - theta_obs) >= (cs.pi / 2), True, False)
skip_due_todir_and_vel = cs.if_else(
obs_driving_towards, False, cs.if_else(obs_faster_than_ego, True,
False))
deactivate_activate_cone = cs.if_else(
passed_obs, True, cs.if_else(skip_due_todir_and_vel, True, False))
c += cs.fmax(0.0,
cs.if_else(deactivate_activate_cone, 0, cs.fmax(0.0, cone)))
# decide what side to drive
# side_obs =cs.sign((x_obs-xkm1)*(yref-ykm1)-(y_obs-ykm1)*(xref-xkm1)) # neg if on left
# s=cs.sign((x-xkm1)*(y_obs-ykm1)-(y-ykm1)*(x_obs-xkm1)) # neg if on left
# #c +=cs.fmax(0.0,s*5)
return c, deactivate_activate_cone
def cone_constraint(c, x_obs, y_obs, r_cone, uk, x, y, theta, passed_obs):
v_obs_x = 0
v_obs_y = 0
r_vec = cs.vertcat(x - x_obs, y - y_obs)
vab = cs.vertcat(
cs.cos(theta) * uk[0] - v_obs_x,
cs.sin(theta) * uk[0] - v_obs_y)
rterm = (cs.norm_2(r_vec))**2
lterm = (cs.norm_2(vab))**2
uterm = (cs.dot(r_vec, vab))**2
cone = (r_cone**2) * lterm - (rterm * lterm - uterm)
skip_due_far_away = cs.if_else(
cs.sqrt((x_obs - x)**2 - (y_obs - y)**2) < 10, False, True)
deactivate_cone = cs.if_else(passed_obs, True, skip_due_far_away)
c += cs.if_else(deactivate_cone, 0, cs.fmax(0.0, cone))
return c, False
def norm2(u):
if (isinstance(u, list) and all(is_numeric(x) for x in u))\
or isinstance(u, np.ndarray):
# if `u` is a numeric vector
return np.linalg.norm(u)
if is_symbolic(u):
return cs.norm_2(u)
raise Exception("Illegal argument")
def test_integration_2(self):
u = cs.SX.sym('u', 3)
xi = cs.SX.sym('xi', 2)
p = cs.SX.sym('p', 2)
f = cs.sin(cs.norm_2(p)) * cs.dot(xi, p)**2 * cs.norm_2(u)
f = 1/f
df = cs.gradient(f, u)
fun = cs.Function('df', [u, xi, p], [df])
transpiler = cc.CasadiRustTranspiler(fun, 'xcst_panda')
transpiler.transpile()
self.assertTrue(transpiler.compile())
(u, xi, p) = ([1, 2, 3], [4, 5], [6, 7])
expected = fun(u, xi, p)
result = transpiler.call_rust(u, xi, p)
for i in range(3):
self.assertAlmostEqual(expected[i], result[i], 8)
def test_second_order_cone_jacobian(self):
soc = og.constraints.SecondOrderCone()
# Important note: the second-order cone constraint does not work with cs.MX
# An exception will be raised if the user provides an SX
u = cs.MX.sym('u', 3)
sq_dist = soc.distance_squared(u)
sq_dist_jac = cs.jacobian(sq_dist, u)
sq_dist_jac_fun = cs.Function('sq_dist_jac', [u], [sq_dist_jac])
v = sq_dist_jac_fun([0., 0., 0.])
for i in range(3):
self.assertFalse(math.isnan(v[i]), "v[i] is NaN")
self.assertAlmostEqual(0, cs.norm_2(v), 12)
def __test_vector_function(self, function, filename):
u = cs.SX.sym('u', 3)
xi = cs.SX.sym('xi', 2)
p = cs.SX.sym('p', 4)
f = cs.norm_2(p) * cs.norm_1(xi) * function(u)
fun = cs.Function('f', [u, xi, p], [f])
transpiler = cc.CasadiRustTranspiler(casadi_function=fun,
function_alias=filename,
rust_dir='tests/rust',
c_dir='tests/c')
transpiler.transpile()
self.assertTrue(transpiler.compile())
(u, xi, p) = ([0.1, 0.2, 0.3], [4, 5], [6, 7, 8, 9])
expected = fun(u, xi, p)
result = transpiler.call_rust(u, xi, p)
self.assertAlmostEqual(expected, result[0], 12)
