Obstacle avoidance

Obstacle avoidance#

In this notebook, we solve an obstacle avoidance problem: finding a smooth path from start to goal while avoiding circular obstacles.

Features used:

  • Var for 2D waypoint positions

  • @jaxls.Cost.factory for smoothness and anchor costs

  • Inequality constraints (constraint_leq_zero): obstacle avoidance

  • Augmented Lagrangian solver for constrained optimization

Hide code cell source

 
import sys
from loguru import logger

logger.remove()
logger.add(sys.stdout, format="<level>{level: <8}</level> | {message}");

import jax
import jax.numpy as jnp
import jaxls

Problem setup#

Plan a trajectory with 20 waypoints from start to goal, avoiding two obstacles:

# Trajectory parameters.
n_waypoints = 20
start = jnp.array([0.0, 0.0])
goal = jnp.array([10.0, 0.0])

# Obstacles: (center_x, center_y, radius)
obstacles = [
    (3.5, 1.0, 1.5),
    (6.5, -1.0, 1.5),
]

Variables and costs#

Variable instances are PyTrees. We define waypoints and cost functions:

class WaypointVar(jaxls.Var[jax.Array], default_factory=lambda: jnp.zeros(2)):
    """2D waypoint position."""


# Batched variable creation.
waypoint_vars = WaypointVar(id=jnp.arange(n_waypoints))

@jaxls.Cost.factory
def smoothness_cost(
    vals: jaxls.VarValues,
    var_prev: WaypointVar,
    var_curr: WaypointVar,
    var_next: WaypointVar,
) -> jax.Array:
    """Penalize acceleration (second derivative)."""
    accel = vals[var_prev] - 2 * vals[var_curr] + vals[var_next]
    return accel * 5.0  # Weight for smoothness


@jaxls.Cost.factory
def anchor_cost(
    vals: jaxls.VarValues,
    var: WaypointVar,
    target: jax.Array,
) -> jax.Array:
    """Pin waypoint to target position."""
    return (vals[var] - target) * 10.0  # Strong weight


@jaxls.Cost.factory(kind="constraint_leq_zero")
def obstacle_constraint(
    vals: jaxls.VarValues,
    var: WaypointVar,
    center: jax.Array,
    radius: float,
) -> jax.Array:
    """Stay outside obstacle: ||p - center|| >= radius."""
    diff = vals[var] - center
    dist = jnp.sqrt(jnp.sum(diff**2) + 1e-6)
    # Constraint: radius - dist <= 0 (i.e., dist >= radius)
    return jnp.array([radius - dist])

Problem construction#

# Indices for smoothness costs (interior waypoints)
smooth_prev = jnp.arange(0, n_waypoints - 2)  # 0, 1, ..., n-3
smooth_curr = jnp.arange(1, n_waypoints - 1)  # 1, 2, ..., n-2
smooth_next = jnp.arange(2, n_waypoints)  # 2, 3, ..., n-1

# Obstacle constraint: each waypoint x each obstacle.
n_obstacles = len(obstacles)
waypoint_ids_for_obs = jnp.tile(jnp.arange(n_waypoints), n_obstacles)
obstacle_centers = jnp.array([[cx, cy] for cx, cy, r in obstacles])
obstacle_radii = jnp.array([r for cx, cy, r in obstacles])
# Repeat each obstacle for all waypoints.
centers_repeated = jnp.repeat(obstacle_centers, n_waypoints, axis=0)
radii_repeated = jnp.repeat(obstacle_radii, n_waypoints)

# Build costs using batched construction.
costs: list[jaxls.Cost] = [
    # Anchor start and goal.
    anchor_cost(WaypointVar(id=0), start),
    anchor_cost(WaypointVar(id=n_waypoints - 1), goal),
    # Smoothness costs (batched)
    smoothness_cost(
        WaypointVar(id=smooth_prev),
        WaypointVar(id=smooth_curr),
        WaypointVar(id=smooth_next),
    ),
    # Obstacle avoidance constraints (batched)
    obstacle_constraint(
        WaypointVar(id=waypoint_ids_for_obs),
        centers_repeated,
        radii_repeated,
    ),
]

print(f"Created {len(costs)} batched cost objects")

Created 4 batched cost objects

# Initialize with straight line (will collide with obstacles)
t = jnp.linspace(0, 1, n_waypoints)
initial_positions = start + t[:, None] * (goal - start)  # (n_waypoints, 2)

initial_vals = jaxls.VarValues.make([waypoint_vars.with_value(initial_positions)])

# Create the problem.
problem = jaxls.LeastSquaresProblem(costs, [waypoint_vars])

# Visualize the problem structure structure.
problem.show()

# Analyze the problem.
problem = problem.analyze()

INFO     | Building optimization problem with 60 terms and 20 variables: 20 costs, 0 eq_zero, 40 leq_zero, 0 geq_zero
INFO     | Vectorizing group with 2 costs, 1 variables each: anchor_cost
INFO     | Vectorizing constraint group with 40 constraints (constraint_leq_zero), 1 variables each: augmented_obstacle_constraint
INFO     | Vectorizing group with 18 costs, 3 variables each: smoothness_cost

Solving#

solution = problem.solve(initial_vals)

INFO     | Augmented Lagrangian: initial snorm=4.8317e-01, csupn=4.8317e-01, max_rho=1.0706e+01, constraint_dim=40
INFO     |  step #0: cost=0.0000 lambd=0.0005 inexact_tol=1.0e-02
INFO     |      - anchor_cost(2): 0.00000 (avg 0.00000)
INFO     |      - augmented_obstacle_constraint(40): 11.46163 (avg 0.28654)
INFO     |      - smoothness_cost(18): 0.00000 (avg 0.00000)
INFO     |      accepted=True ATb_norm=1.19e+01 cost_prev=11.4616 cost_new=0.9302
INFO     |  step #1: cost=0.8331 lambd=0.0003 inexact_tol=1.0e-02
INFO     |      - anchor_cost(2): 0.00119 (avg 0.00030)
INFO     |      - augmented_obstacle_constraint(40): 0.09713 (avg 0.00243)
INFO     |      - smoothness_cost(18): 0.83191 (avg 0.02311)
INFO     |      accepted=True ATb_norm=9.98e-01 cost_prev=0.9302 cost_new=0.8834
INFO     |  step #2: cost=0.7775 lambd=0.0001 inexact_tol=6.3e-03
INFO     |      - anchor_cost(2): 0.00125 (avg 0.00031)
INFO     |      - augmented_obstacle_constraint(40): 0.10585 (avg 0.00265)
INFO     |      - smoothness_cost(18): 0.77627 (avg 0.02156)
INFO     |      accepted=True ATb_norm=4.75e-02 cost_prev=0.8834 cost_new=0.8828
INFO     |  step #3: cost=0.7712 lambd=0.0001 inexact_tol=2.0e-03
INFO     |      - anchor_cost(2): 0.00122 (avg 0.00030)
INFO     |      - augmented_obstacle_constraint(40): 0.11158 (avg 0.00279)
INFO     |      - smoothness_cost(18): 0.76997 (avg 0.02139)
INFO     |      accepted=True ATb_norm=6.31e-03 cost_prev=0.8828 cost_new=0.8828
INFO     |  AL update: snorm=7.0361e-02, csupn=7.0361e-02, max_rho=1.0706e+01
INFO     |  step #4: cost=0.7707 lambd=0.0000 inexact_tol=2.0e-03
INFO     |      - anchor_cost(2): 0.00122 (avg 0.00030)
INFO     |      - augmented_obstacle_constraint(40): 0.44837 (avg 0.01121)
INFO     |      - smoothness_cost(18): 0.76945 (avg 0.02137)
INFO     |      accepted=True ATb_norm=1.18e+00 cost_prev=1.2190 cost_new=1.1491
INFO     |  step #5: cost=0.9132 lambd=0.0000 inexact_tol=2.0e-03
INFO     |      - anchor_cost(2): 0.00134 (avg 0.00034)
INFO     |      - augmented_obstacle_constraint(40): 0.23599 (avg 0.00590)
INFO     |      - smoothness_cost(18): 0.91182 (avg 0.02533)
INFO     |      accepted=True ATb_norm=1.96e-01 cost_prev=1.1491 cost_new=1.1436
INFO     |  step #6: cost=0.9509 lambd=0.0000 inexact_tol=2.0e-03
INFO     |      - anchor_cost(2): 0.00148 (avg 0.00037)
INFO     |      - augmented_obstacle_constraint(40): 0.19274 (avg 0.00482)
INFO     |      - smoothness_cost(18): 0.94939 (avg 0.02637)
INFO     |      accepted=True ATb_norm=1.85e-02 cost_prev=1.1436 cost_new=1.1435
INFO     |  step #7: cost=0.9518 lambd=0.0000 inexact_tol=2.0e-03
INFO     |      - anchor_cost(2): 0.00148 (avg 0.00037)
INFO     |      - augmented_obstacle_constraint(40): 0.19162 (avg 0.00479)
INFO     |      - smoothness_cost(18): 0.95036 (avg 0.02640)
INFO     |      accepted=True ATb_norm=4.87e-03 cost_prev=1.1435 cost_new=1.1435
INFO     |  AL update: snorm=2.4075e-02, csupn=2.4075e-02, max_rho=1.0706e+01
INFO     |  step #8: cost=0.9522 lambd=0.0000 inexact_tol=2.0e-03
INFO     |      - anchor_cost(2): 0.00148 (avg 0.00037)
INFO     |      - augmented_obstacle_constraint(40): 0.30073 (avg 0.00752)
INFO     |      - smoothness_cost(18): 0.95070 (avg 0.02641)
INFO     |      accepted=True ATb_norm=3.96e-01 cost_prev=1.2529 cost_new=1.2444
INFO     |  step #9: cost=1.0277 lambd=0.0000 inexact_tol=2.0e-03
INFO     |      - anchor_cost(2): 0.00161 (avg 0.00040)
INFO     |      - augmented_obstacle_constraint(40): 0.21676 (avg 0.00542)
INFO     |      - smoothness_cost(18): 1.02608 (avg 0.02850)
INFO     |      accepted=True ATb_norm=4.91e-03 cost_prev=1.2444 cost_new=1.2444
INFO     |  AL update: snorm=6.1830e-03, csupn=6.1830e-03, max_rho=1.0706e+01
INFO     |  step #10: cost=1.0277 lambd=0.0000 inexact_tol=1.4e-04
INFO     |      - anchor_cost(2): 0.00161 (avg 0.00040)
INFO     |      - augmented_obstacle_constraint(40): 0.24424 (avg 0.00611)
INFO     |      - smoothness_cost(18): 1.02605 (avg 0.02850)
INFO     |      accepted=True ATb_norm=1.00e-01 cost_prev=1.2719 cost_new=1.2712
INFO     |  step #11: cost=1.0498 lambd=0.0000 inexact_tol=1.4e-04
INFO     |      - anchor_cost(2): 0.00164 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.22144 (avg 0.00554)
INFO     |      - smoothness_cost(18): 1.04813 (avg 0.02911)
INFO     |      accepted=True ATb_norm=5.15e-04 cost_prev=1.2712 cost_new=1.2712
INFO     |  AL update: snorm=1.0778e-03, csupn=1.0778e-03, max_rho=1.0706e+01
INFO     |  step #12: cost=1.0498 lambd=0.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00164 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.22616 (avg 0.00565)
INFO     |      - smoothness_cost(18): 1.04813 (avg 0.02911)
INFO     |      accepted=True ATb_norm=1.75e-02 cost_prev=1.2759 cost_new=1.2759
INFO     |  step #13: cost=1.0536 lambd=0.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.22226 (avg 0.00556)
INFO     |      - smoothness_cost(18): 1.05200 (avg 0.02922)
INFO     |      accepted=False ATb_norm=1.61e-04 cost_prev=1.2759 cost_new=1.2759
INFO     |  AL update: snorm=1.8787e-04, csupn=1.8787e-04, max_rho=1.0706e+01
INFO     |  step #14: cost=1.0536 lambd=0.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.22308 (avg 0.00558)
INFO     |      - smoothness_cost(18): 1.05200 (avg 0.02922)
INFO     |      accepted=True ATb_norm=3.06e-03 cost_prev=1.2767 cost_new=1.2767
INFO     |  AL update: snorm=3.2902e-05, csupn=3.2902e-05, max_rho=1.0706e+01
INFO     |  step #15: cost=1.0543 lambd=0.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.22255 (avg 0.00556)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |  step #16: cost=1.0543 lambd=0.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.22255 (avg 0.00556)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |  step #17: cost=1.0543 lambd=0.0003 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.22255 (avg 0.00556)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |  step #18: cost=1.0543 lambd=0.0051 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.22255 (avg 0.00556)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |  step #19: cost=1.0543 lambd=0.1638 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.22255 (avg 0.00556)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |  step #20: cost=1.0543 lambd=10.4858 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.22255 (avg 0.00556)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |  step #21: cost=1.0543 lambd=1342.1772 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.22255 (avg 0.00556)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=True ATb_norm=5.89e-04 cost_prev=1.2769 cost_new=1.2769
INFO     |  AL update: snorm=3.2663e-05, csupn=3.2663e-05, max_rho=4.2824e+01
INFO     |  step #22: cost=1.0543 lambd=671.0886 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.05578 (avg 0.00139)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |  step #23: cost=1.0543 lambd=2684.3545 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.05578 (avg 0.00139)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |  step #24: cost=1.0543 lambd=21474.8359 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.05578 (avg 0.00139)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |  step #25: cost=1.0543 lambd=343597.3750 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.05578 (avg 0.00139)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |  step #26: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.05578 (avg 0.00139)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=2.60e-03 cost_prev=1.1101 cost_new=1.1101
INFO     |  AL update: snorm=3.2663e-05, csupn=3.2663e-05, max_rho=1.7129e+02
INFO     |  step #27: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #28: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #29: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #30: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #31: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #32: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #33: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #34: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #35: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #36: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #37: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #38: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #39: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #40: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #41: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #42: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #43: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #44: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #45: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #46: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #47: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #48: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #49: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #50: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #51: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #52: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #53: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #54: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #55: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #56: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #57: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #58: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #59: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #60: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #61: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #62: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #63: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #64: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #65: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #66: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #67: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #68: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #69: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #70: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #71: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #72: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #73: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #74: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #75: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #76: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #77: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #78: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #79: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #80: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #81: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #82: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #83: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #84: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #85: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #86: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #87: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #88: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #89: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #90: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #91: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #92: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #93: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #94: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #95: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #96: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #97: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #98: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     |  step #99: cost=1.0543 lambd=1000000.0000 inexact_tol=2.4e-05
INFO     |      - anchor_cost(2): 0.00165 (avg 0.00041)
INFO     |      - augmented_obstacle_constraint(40): 0.01409 (avg 0.00035)
INFO     |      - smoothness_cost(18): 1.05267 (avg 0.02924)
INFO     |      accepted=False ATb_norm=1.10e-02 cost_prev=1.0684 cost_new=1.0684
INFO     | Terminated @ iteration #100: cost=1.0543 criteria=[0 0 0], term_deltas=0.0e+00,7.7e-03,2.1e-10 (solved in 0.4790 sec)

Visualization#

Hide code cell source

 
import plotly.graph_objects as go
from plotly.subplots import make_subplots
from IPython.display import HTML


def get_trajectory_traces(
    vals: jaxls.VarValues, name_prefix: str = ""
) -> list[go.Scatter]:
    """Get Plotly traces for trajectory visualization.

    Args:
        vals: Variable values containing waypoint positions
        name_prefix: Prefix for trace names

    Returns:
        List of Plotly Scatter traces for path, start, and goal markers
    """
    traj = vals[waypoint_vars]  # (n_waypoints, 2)
    traj_x = [float(x) for x in traj[:, 0]]
    traj_y = [float(y) for y in traj[:, 1]]

    return [
        # Trajectory line and points.
        go.Scatter(
            x=traj_x,
            y=traj_y,
            mode="lines+markers",
            line=dict(color="steelblue", width=2),
            marker=dict(size=6, color="steelblue"),
            name=f"{name_prefix}Path",
            hovertemplate="(%{x:.2f}, %{y:.2f})<extra></extra>",
        ),
        # Start marker.
        go.Scatter(
            x=[traj_x[0]],
            y=[traj_y[0]],
            mode="markers",
            marker=dict(size=14, color="green", symbol="circle"),
            name=f"{name_prefix}Start",
            hovertemplate="Start (%{x:.2f}, %{y:.2f})<extra></extra>",
        ),
        # Goal marker.
        go.Scatter(
            x=[traj_x[-1]],
            y=[traj_y[-1]],
            mode="markers",
            marker=dict(size=14, color="orange", symbol="square"),
            name=f"{name_prefix}Goal",
            hovertemplate="Goal (%{x:.2f}, %{y:.2f})<extra></extra>",
        ),
    ]


def get_obstacle_shapes() -> list[dict]:
    """Get Plotly shapes for obstacles.

    Returns:
        List of Plotly shape dictionaries for circular obstacles
    """
    return [
        dict(
            type="circle",
            xref="x",
            yref="y",
            x0=cx - r,
            y0=cy - r,
            x1=cx + r,
            y1=cy + r,
            fillcolor="rgba(255, 99, 71, 0.3)",
            line=dict(color="tomato", width=2),
        )
        for cx, cy, r in obstacles
    ]


fig = make_subplots(
    rows=1,
    cols=2,
    subplot_titles=("Initial (straight line)", "Optimized (avoids obstacles)"),
)

# Add trajectory traces.
for trace in get_trajectory_traces(initial_vals):
    fig.add_trace(trace, row=1, col=1)
for trace in get_trajectory_traces(solution):
    fig.add_trace(trace, row=1, col=2)

# Add obstacle shapes to both subplots.
obstacle_shapes = get_obstacle_shapes()
for shape in obstacle_shapes:
    fig.add_shape({**shape, "xref": "x", "yref": "y"}, row=1, col=1)
    fig.add_shape({**shape, "xref": "x2", "yref": "y2"}, row=1, col=2)

# Consistent axis ranges for both subplots.
fig.update_xaxes(title_text="x", range=[-1, 11], row=1, col=1)
fig.update_xaxes(title_text="x", range=[-1, 11], row=1, col=2)
fig.update_yaxes(
    title_text="y", range=[-4, 4], scaleanchor="x", scaleratio=1, row=1, col=1
)
fig.update_yaxes(
    title_text="y", range=[-4, 4], scaleanchor="x2", scaleratio=1, row=1, col=2
)
fig.update_layout(height=400, showlegend=False, margin=dict(t=40, b=40, l=40, r=40))
HTML(fig.to_html(full_html=False, include_plotlyjs="cdn"))

The optimizer found a smooth trajectory that avoids both obstacles while connecting start to goal. The inequality constraints keep waypoints outside the obstacle regions.

For solver configuration options, see jaxls.TrustRegionConfig and jaxls.AugmentedLagrangianConfig.

Contents