Skip to content

Tighten limits/collision geometry by success_tolerance #79

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Nikos-d wants to merge 18 commits into
base: main
Choose a base branch
from
Open

Tighten limits/collision geometry by success_tolerance #79

Show file tree
Hide file tree
Changes from 16 commits
Commits
Show all changes
18 commits
Select commit Hold shift + click to select a range
bb4d97a
Tighten limits/collision geometry by success_tolerance instead of rel…
Nikos-d Aug 25, 2026
070a53c
style: apply clang-format
Nikos-d Aug 25, 2026
00683e5
fix: don't tighten limits a pinned boundary condition is evaluated ag…
Nikos-d Aug 29, 2026
1c4cb30
style: apply clang-format
Nikos-d Aug 29, 2026
69190c0
test: pin the tightening contract
Nikos-d Aug 29, 2026
3af007f
style: apply clang-format
Nikos-d Aug 29, 2026
603138c
test: assert the retry-loop regression, not first-try success
Nikos-d Aug 29, 2026
4ae580d
test: deterministic initial guess; no test may fail periodically
Nikos-d Aug 29, 2026
ea9afc7
Merge remote-tracking branch 'origin/nikos/tight_validation' into nik…
Nikos-d Aug 29, 2026
7dd4599
style: apply clang-format
Nikos-d Aug 29, 2026
bea7501
docs: keep downstream benchmark specifics out of the library
Nikos-d Aug 29, 2026
39d7e31
docs: document collision_buffer; trim comments
Nikos-d Aug 29, 2026
d2e4764
Merge remote-tracking branch 'origin/nikos/tight_validation' into nik…
Nikos-d Aug 29, 2026
928fa11
style: apply clang-format
Nikos-d Aug 29, 2026
4813d91
Merge branch 'main' into nikos/tight_validation
gallantandre Sep 5, 2026
2716e4b
style: apply clang-format
gallantandre Sep 5, 2026
5d3eac1
Addressed PR comments
Nikos-d Sep 15, 2026
25cbce4
style: apply clang-format
Nikos-d Sep 15, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 13 additions & 6 deletions blast/blast_optimization.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -119,15 +119,22 @@ struct Optimization {
Objective objective;
Matrix task;
World world;
real trajectory_start_time = 0.0;
real success_tolerance = 0.01; // constraint violation after optimization that is still considered a success
bool tighten_for_tolerance = true; // false skips the tightening (ablation only: an accepted
// solve may then violate the real limits by up to
// success_tolerance)
real collision_buffer = 0.001; // how close, in metres, the trajectory may come to
// contact. success_tolerance is dimensionless and
// cannot express a distance; 0 falls back to it
real collision_scale = 1.0; // derived, do not set: success_tolerance /
// collision_buffer while tightened, 1 otherwise
int max_tries = 1; // Maximum number of tries in the optimization loop.
int max_eval = 1000; // Maximum number of function evaluations for a single NLopt call.
real max_time = 30.0; // Maximum time (seconds) for a single NLopt call.

std::array<BoundingVolumeHierarchy<AABBPair>, MAX_CAPSULES> time_bounding_volume_hierarchies; // should be std::array<> ? Never changes size

real trajectory_start_time = 0.0;
real success_tolerance = 0.01; // constraint violation after optimization that is still considered a success
int max_tries = 1; // Maximum number of tries in the optimization loop.
int max_eval = 1000; // Maximum number of function evaluations for a single NLopt call.
real max_time = 30.0; // Maximum time (seconds) for a single NLopt call.

void* custom_data;

Array lb;
Expand Down
44 changes: 22 additions & 22 deletions blast/optimization/constraints.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -205,7 +205,7 @@ inline blast_fn void constraints_and_gradients_with_segments(const Array& x, Opt
#endif
const auto J_tool = get_J_tool(&opt, manip_data); // todo: clean up get_J_tool to a get_tool_speed
const auto tool_speed = norm(get_J_tool(&opt, manip_data) * v);
if (const auto c = bound_constraint(tool_speed, 0.0, tool_speed_max);
if (const auto c = abs_constraint(tool_speed, tool_speed_max);
c > max_tool_speed_constraints) {
max_tool_speed_constraints = c;
max_tool_index = point_in_segment;
Expand All @@ -219,7 +219,7 @@ inline blast_fn void constraints_and_gradients_with_segments(const Array& x, Opt
#endif
// check every internal collision
auto self_collision_distances = get_internal_collisions(opt.manip, manip_data);
if (const auto c = -min(self_collision_distances);
if (const auto c = -min(self_collision_distances) * opt.collision_scale;
c > max_internal_col_constraints) {
max_internal_col_constraints = c;
max_internal_collision_index = point_in_segment;
Expand Down Expand Up @@ -335,7 +335,7 @@ inline blast_fn void constraints_and_gradients_with_segments(const Array& x, Opt
count++;
}

dist_min = -dist_min; // negative distance is positive constraint
dist_min = -dist_min * opt.collision_scale; // negative distance is positive constraint

// update worst position for the current capsule if necessary
if (dist_min > max_col_constraints[capsule_id]) {
Expand Down Expand Up @@ -605,7 +605,7 @@ inline blast_fn void constraints_and_gradients_with_segments(const Array& x, Opt
forward_kinematics(opt.manip, manip_data, p_plus);
const auto J_tool_p = get_J_tool(&opt, manip_data);
const auto tool_speed_p = norm(J_tool_p * v_plus);
const auto new_constraint_p = bound_constraint(tool_speed_p, 0.0, tool_speed_max);
const auto new_constraint_p = abs_constraint(tool_speed_p, tool_speed_max);
// partial difference d(tool_speed)/dp
const real dtool_speed_dp = (new_constraint_p - max_tool_speed_constraints) / eps;
p_plus[j] = p[j]; // reset finite difference
Expand All @@ -614,7 +614,7 @@ inline blast_fn void constraints_and_gradients_with_segments(const Array& x, Opt
v_plus[j] += eps;
const auto J_tool_v = get_J_tool(&opt, manip_data);
const auto tool_speed_v = norm(J_tool_v * v_plus);
const auto new_constraint_v = bound_constraint(tool_speed_v, 0.0, tool_speed_max);
const auto new_constraint_v = abs_constraint(tool_speed_v, tool_speed_max);
const real dtool_speed_dv = (new_constraint_v - max_tool_speed_constraints) / eps;
v_plus[j] = v[j];

Expand Down Expand Up @@ -656,7 +656,7 @@ inline blast_fn void constraints_and_gradients_with_segments(const Array& x, Opt
// recompute internal collisions at the worst point in segment
forward_kinematics(opt.manip, manip_data, p_plus);
compute_collision_model(opt.manip, manip_data);
const auto new_internal_collision_constraint = max(-get_internal_collisions(opt.manip, manip_data));
const auto new_internal_collision_constraint = max(-get_internal_collisions(opt.manip, manip_data)) * opt.collision_scale;
// partial difference d(internal_collision)/dp
const real dint_coll_dp = (new_internal_collision_constraint - max_internal_col_constraints) / eps;

Expand Down Expand Up @@ -719,7 +719,7 @@ inline blast_fn void constraints_and_gradients_with_segments(const Array& x, Opt
}
}

distance_plus = -distance_plus; // negative distance is positive constraint
distance_plus = -distance_plus * opt.collision_scale; // negative distance is positive constraint

// partial difference d(collision)/dp
const real dcoll_dp = (distance_plus - max_col_constraints[capsule_id]) / eps;
Expand Down Expand Up @@ -2140,14 +2140,14 @@ inline blast_fn void compute_constraints(real* result, const Array& x, Optimizat
#endif
auto J_tool = get_J_tool(opt, manip_data);
real tool_speed = norm(J_tool * opt->bspline.traj.vel.col(i));
*moving_result++ = bound_constraint(tool_speed, 0.0, opt->manip.tool_speed_max);
*moving_result++ = abs_constraint(tool_speed, opt->manip.tool_speed_max);
}

if (opt->constraints.self_collisions) {
#if BLAST_TRACE_LEVEL >= 3
PROFILE_SCOPE("SelfCollisions");
#endif
auto tmp_coll = max(-get_internal_collisions(opt->manip, manip_data));
auto tmp_coll = max(-get_internal_collisions(opt->manip, manip_data)) * opt->collision_scale;
// for (u32 j = 0; j < tmp_coll.size; j++)
*moving_result++ = tmp_coll; //*std::abs(tmp_coll[j]);
}
Expand Down Expand Up @@ -2191,7 +2191,7 @@ inline blast_fn void compute_constraints(real* result, const Array& x, Optimizat
count++;
}

dist_min = -dist_min; // negative distance is positive constraint
dist_min = -dist_min * opt->collision_scale; // negative distance is positive constraint

// update worst position for the current capsule if necessary
if (dist_min > max_col_constraints[capsule_id]) {
Expand Down Expand Up @@ -2359,7 +2359,7 @@ inline blast_fn bool validate_task(Optimization* opt) {
if (constraints.tool_speed) {
auto J_tool = get_J_tool(opt, manip_data);
real tool_speed = norm(J_tool * vel.col(i));
result = bound_constraint(tool_speed, -INF_REAL, manip.tool_speed_max);
result = abs_constraint(tool_speed, manip.tool_speed_max);
if (result > 0) {
std::cout << "Tool speed outside bounds." << std::endl;
return false;
Expand Down Expand Up @@ -2539,14 +2539,14 @@ inline void compute_constraints_with_analytical_pva(ConstraintPerPoint& constrai
#endif
auto J_tool = get_J_tool(opt, manip_data);
real tool_speed = norm(J_tool * opt->bspline.traj.vel.col(i));
constraints.tool_constraint[i - opt->bspline.lower_bounds[x_idx]] = bound_constraint(tool_speed, 0.0, opt->manip.tool_speed_max);
constraints.tool_constraint[i - opt->bspline.lower_bounds[x_idx]] = abs_constraint(tool_speed, opt->manip.tool_speed_max);
}

if (opt->constraints.self_collisions) {
#if BLAST_TRACE_LEVEL >= 3
ZoneScopedN("SelfCollisions");
#endif
auto tmp_coll = max(-get_internal_collisions(opt->manip, manip_data));
auto tmp_coll = max(-get_internal_collisions(opt->manip, manip_data)) * opt->collision_scale;
// for (u32 j = 0; j < tmp_coll.size; j++)
constraints.self_collision_constraint[i - opt->bspline.lower_bounds[x_idx]] = tmp_coll; //*std::abs(tmp_coll[j]);
}
Expand Down Expand Up @@ -2591,7 +2591,7 @@ inline void compute_constraints_with_analytical_pva(ConstraintPerPoint& constrai
count++;
}

dist_min = -dist_min; // negative distance is positive constraint
dist_min = -dist_min * opt->collision_scale; // negative distance is positive constraint

// update worst position for the current capsule if necessary
if (dist_min > max_col_constraints[capsule_id]) {
Expand Down Expand Up @@ -2739,15 +2739,15 @@ blast_fn void compute_constraints_with_analytical_dynamics(real* result, Array&
#endif
auto J_tool = get_J_tool(opt, manip_data);
real tool_speed = norm(J_tool * vel);
tool_constraint = bound_constraint(tool_speed, 0.0, opt->manip.tool_speed_max);
tool_constraint = abs_constraint(tool_speed, opt->manip.tool_speed_max);
*moving_result++ = tool_constraint;
}

if (opt->constraints.self_collisions) {
#if BLAST_TRACE_LEVEL >= 3
ZoneScopedN("SelfCollisions");
#endif
self_collision_constraint = max(-get_internal_collisions(opt->manip, manip_data));
self_collision_constraint = max(-get_internal_collisions(opt->manip, manip_data)) * opt->collision_scale;
*moving_result++ = self_collision_constraint;
}

Expand Down Expand Up @@ -2801,7 +2801,7 @@ blast_fn void compute_constraints_with_analytical_dynamics(real* result, Array&
count++;
}

dist_min = -dist_min; // negative distance is positive constraint
dist_min = -dist_min * opt->collision_scale; // negative distance is positive constraint

// update worst position for the current capsule if necessary
if (dist_min > max_col_constraints[capsule_id]) {
Expand Down Expand Up @@ -2841,13 +2841,13 @@ blast_fn void compute_constraints_with_analytical_dynamics(real* result, Array&
if (opt->constraints.tool_speed) {
auto J_tool_plus = get_J_tool(opt, manip_data);
real tool_speed_plus = norm(J_tool_plus * vel_plus);
auto tool_constraint_plus = bound_constraint(tool_speed_plus, 0.0, opt->manip.tool_speed_max);
auto tool_constraint_plus = abs_constraint(tool_speed_plus, opt->manip.tool_speed_max);
dtool_dp(j, i) = (tool_constraint_plus - tool_constraint) / eps;
}

compute_collision_model(opt->manip, manip_data);
if (opt->constraints.self_collisions) {
auto self_collision_constraint_plus = max(-get_internal_collisions(opt->manip, manip_data));
auto self_collision_constraint_plus = max(-get_internal_collisions(opt->manip, manip_data)) * opt->collision_scale;
dselfcol_dp(j, i) = (self_collision_constraint_plus - self_collision_constraint) / eps;
}

Expand All @@ -2870,7 +2870,7 @@ blast_fn void compute_constraints_with_analytical_dynamics(real* result, Array&
break;
}
}
distance_plus = -distance_plus;
distance_plus = -distance_plus * opt->collision_scale;
// auto external_collisions_plus = -test_collisions_per_point(manip_data.capsule_list, &(opt->world));
dcol_dp[i].resize(n_capsules, joints);
dcol_dp[i](capsule_id, j) = (distance_plus - max_col_constraints[capsule_id]) / eps;
Expand All @@ -2896,7 +2896,7 @@ blast_fn void compute_constraints_with_analytical_dynamics(real* result, Array&
if (opt->constraints.tool_speed) {
auto J_tool_plus = get_J_tool(opt, manip_data);
real tool_speed_plus = norm(J_tool_plus * vel_plus);
auto tool_constraint_plus = bound_constraint(tool_speed_plus, 0.0, opt->manip.tool_speed_max);
auto tool_constraint_plus = abs_constraint(tool_speed_plus, opt->manip.tool_speed_max);
dtool_dv(j, i) = (tool_constraint_plus - tool_constraint) / eps;
}

Expand Down Expand Up @@ -2980,7 +2980,7 @@ inline blast_fn void compute_constraints_per_point(real* result, real& external_
#endif
auto J_tool = get_J_tool(opt, manip_data);
real tool_speed = norm(J_tool * opt->bspline.traj.vel.col(i));
moving_result[0] = bound_constraint(tool_speed, 0.0, opt->manip.tool_speed_max);
moving_result[0] = abs_constraint(tool_speed, opt->manip.tool_speed_max);
moving_result++;
}

Expand Down
Loading
Loading