Skip to content

Commit

Permalink
WIP: getting snopt version ready to PR
Browse files Browse the repository at this point in the history
  • Loading branch information
@RussTedrake
RussTedrake committed Jul 25, 2021
1 parent 9a0d063 commit d6908d5
Show file tree
Hide file tree
Showing 3 changed files with 73 additions and 68 deletions.
64 changes: 32 additions & 32 deletions geometry/optimization/iris.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -221,37 +221,40 @@ bool FindClosestCollision(const multibody::MultibodyPlant<Expression>& plant,
prog.AddConstraint(X_WA * p_AA.cast<Expression>() ==
X_WB * p_BB.cast<Expression>());

// Help nonlinear optimizers (e.g. SNOPT) avoid trivial local minima at the
// origin.
prog.SetSolverOption(solvers::SnoptSolver::id(), "Print file",
"/tmp/iris_snopt.out");
prog.SetInitialGuess(q, VectorXd::Constant(plant.num_positions(), .01));
prog.SetInitialGuess(p_AA, Vector3d::Constant(.01));
prog.SetInitialGuess(p_BB, Vector3d::Constant(.01));
for (const auto binding : prog.lorentz_cone_constraints()) {
const auto c = binding.evaluator();
prog.AddConstraint(
std::make_shared<solvers::LorentzConeConstraint>(
c->A_dense(), c->b(),
solvers::LorentzConeConstraint::EvalType::kNonconvex),
binding.variables());
prog.RemoveConstraint(binding);
if (solver.solver_id() == solvers::IbexSolver::id()) {
// Use kNonconvex instead of the default kConvexSmooth.
for (const auto binding : prog.lorentz_cone_constraints()) {
const auto c = binding.evaluator();
prog.AddConstraint(
std::make_shared<solvers::LorentzConeConstraint>(
c->A_dense(), c->b(),
solvers::LorentzConeConstraint::EvalType::kNonconvex),
binding.variables());
prog.RemoveConstraint(binding);
}
} else {
// Help nonlinear optimizers (e.g. SNOPT) avoid trivial local minima at the
// origin.
prog.SetInitialGuess(q, VectorXd::Constant(plant.num_positions(), .01));
prog.SetInitialGuess(p_AA, Vector3d::Constant(.01));
prog.SetInitialGuess(p_BB, Vector3d::Constant(.01));
}

solvers::MathematicalProgramResult result = solver.Solve(prog);
if (result.is_success()) {
*closest = result.GetSolution(q);
return true;
}
std::cout << prog << std::endl;
//std::cout << prog << std::endl;
return false;
}

} // namespace

HPolyhedron IrisInConfigurationSpace(
const multibody::MultibodyPlant<Expression>& plant,
const QueryObject<double>& query_object,
const multibody::MultibodyPlant<double>& plant,
const SceneGraph<double>& scene_graph,
const systems::Context<double>& root_context,
const Eigen::Ref<const Eigen::VectorXd>& sample,
const IrisOptions& options) {
const int nq = plant.num_positions();
Expand All @@ -268,6 +271,11 @@ HPolyhedron IrisInConfigurationSpace(
const double kEpsilonEllipsoid = 1e-2;
Hyperellipsoid E = Hyperellipsoid::MakeHypersphere(kEpsilonEllipsoid, sample);

std::unique_ptr<multibody::MultibodyPlant<symbolic::Expression>>
symbolic_plant = systems::System<double>::ToSymbolic(plant);
auto query_object =
scene_graph.get_query_output_port().Eval<QueryObject<double>>(
scene_graph.GetMyContextFromRoot(root_context));
const SceneGraphInspector<double>& inspector = query_object.inspector();

// Make all of the convex sets. TODO(russt): Consider factoring this out so
Expand All @@ -285,7 +293,7 @@ HPolyhedron IrisInConfigurationSpace(
maker.set_geometry_id(geom_id);
inspector.GetShape(geom_id).Reify(&maker, &temp_set);
sets.emplace(geom_id, std::move(temp_set));
bodies.emplace(geom_id, *plant.GetBodyFromFrameId(frame_id));
bodies.emplace(geom_id, *symbolic_plant->GetBodyFromFrameId(frame_id));
}

// TODO(russt): As a surrogate for the true objective, we could use convex
Expand All @@ -309,10 +317,10 @@ HPolyhedron IrisInConfigurationSpace(
int iteration = 0;
MatrixXd tangent_matrix;

//solvers::IbexSolver solver;
// solvers::IbexSolver solver;
solvers::SnoptSolver solver;
DRAKE_DEMAND(solver.is_available() && solver.is_enabled());
auto context = plant.CreateDefaultContext();
auto symbolic_context = symbolic_plant->CreateDefaultContext();
VectorXd closest(nq);

while (true) {
Expand All @@ -322,13 +330,10 @@ HPolyhedron IrisInConfigurationSpace(
for (const auto [geomA, geomB] : pairs) {
while (true) {
const bool collision = FindClosestCollision(
plant, bodies.at(geomA), bodies.at(geomB), *sets.at(geomA),
*sets.at(geomB), E, A.topRows(num_constraints),
b.head(num_constraints), solver, context.get(), &closest);
std::cout << "geomA=" << inspector.GetName(geomA)
<< ", geomB=" << inspector.GetName(geomB);
*symbolic_plant, bodies.at(geomA), bodies.at(geomB),
*sets.at(geomA), *sets.at(geomB), E, A.topRows(num_constraints),
b.head(num_constraints), solver, symbolic_context.get(), &closest);
if (collision) {
std::cout << ", closest=" << closest.transpose() << std::endl;
// Add the tangent to the (scaled) ellipsoid at this point as a
// constraint.
if (num_constraints >= A.rows()) {
Expand All @@ -341,13 +346,8 @@ HPolyhedron IrisInConfigurationSpace(
(tangent_matrix * (closest - E.center())).normalized();
b[num_constraints] = A.row(num_constraints) * closest -
options.configuration_space_margin;
std::cout << " "
<< A.row(num_constraints) *
symbolic::MakeVectorVariable(nq, "q")
<< " ≤ " << b[num_constraints] << std::endl;
num_constraints++;
} else {
std::cout << ". No collisions." << std::endl;
break;
}
}
Expand Down
29 changes: 25 additions & 4 deletions geometry/optimization/iris.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,11 +93,32 @@ ConvexSets MakeIrisObstacles(
const QueryObject<double>& query_object,
std::optional<FrameId> reference_frame = std::nullopt);

// TODO(russt): Pass a context of the plant if/when we support setting kinematic
// parameters.
/** A variation of the Iris (Iterative Region Inflation by Semidefinite
programming) algorithm which finds collision-free regions in the *configuration
space* of @p plant. @see Iris for details on the original algorithm.
The possibility of this configuration-space variant was suggested in the
original IRIS paper, but substantial new ideas have been employed here to
address the non-convexity of configuration-space obstacles.
@param plant describes the kinematics of configuration space.
@param scene_graph describes the geometry; it must be connected to @p plant in a
systems::Diagram.
@param root_context is a context of the systems::Diagram containing the
systems::Context for both @p plant and @p scene_graph.
@param sample is a vector of size plant.num_positions() representing the initial
IRIS seed configuration.
@param options provides additional configuration options.
Note: This initial implementation **does not** yet provide a rigorous guarantee
that the returned region is collision free. The certification step will be
contributed in a follow-up PR.
@ingroup geometry_optimization
*/
HPolyhedron IrisInConfigurationSpace(
const multibody::MultibodyPlant<symbolic::Expression>& plant,
const QueryObject<double>& query_object,
const multibody::MultibodyPlant<double>& plant,
const SceneGraph<double>& scene_graph,
const systems::Context<double>& root_context,
const Eigen::Ref<const Eigen::VectorXd>& sample,
const IrisOptions& options = IrisOptions());

Expand Down
48 changes: 16 additions & 32 deletions geometry/optimization/test/iris_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -248,13 +248,8 @@ HPolyhedron IrisFromUrdf(const std::string urdf,
auto diagram = builder.Build();

auto context = diagram->CreateDefaultContext();
auto query_object =
scene_graph.get_query_output_port().Eval<QueryObject<double>>(
scene_graph.GetMyContextFromRoot(*context));

std::unique_ptr<multibody::MultibodyPlant<symbolic::Expression>>
symbolic_plant = systems::System<double>::ToSymbolic(plant);
return IrisInConfigurationSpace(*symbolic_plant, query_object, sample);
return IrisInConfigurationSpace(plant, scene_graph, *context, sample,
options);
}

} // namespace
Expand Down Expand Up @@ -496,40 +491,29 @@ GTEST_TEST(IrisInConfigurationSpaceTest, DoublePendulum) {
fmt::arg("w_plus_one_half", w + .5), fmt::arg("l1", l1),
fmt::arg("l2", l2), fmt::arg("r", r));

std::cout << double_pendulum_urdf << std::endl;

systems::DiagramBuilder<double> builder;
auto [plant, scene_graph] =
multibody::AddMultibodyPlantSceneGraph(&builder, 0.0);
multibody::Parser(&plant).AddModelFromString(double_pendulum_urdf, "urdf");
plant.Finalize();
auto diagram = builder.Build();

auto context = diagram->CreateDefaultContext();

// const double kTol = 1e-5;

// Confirm the configuration space boundary via SceneGraph.
auto& plant_context = plant.GetMyMutableContextFromRoot(context.get());
plant.SetPositions(&plant_context, Vector2d(.5, .5));
auto query_object =
scene_graph.get_query_output_port().Eval<QueryObject<double>>(
scene_graph.GetMyContextFromRoot(*context));
EXPECT_TRUE(query_object.HasCollisions());

const Vector2d sample = Vector2d::Zero();
IrisOptions options;
HPolyhedron region = IrisFromUrdf(double_pendulum_urdf, sample, options);

// Note: You may use this to plot the solution in the desmos graphing
// calculator link above. Just copy each equation in the printed formula into
// a desmos cell. The intersection is the computed region.
const Vector2<symbolic::Expression> xy{symbolic::Variable("x"),
symbolic::Variable("y")};
std::cout << (region.A()*xy <= region.b()) << std::endl;
// const Vector2<symbolic::Expression> xy{symbolic::Variable("x"),
// symbolic::Variable("y")};
// std::cout << (region.A()*xy <= region.b()) << std::endl;

EXPECT_EQ(region.ambient_dimension(), 2);
EXPECT_TRUE(false);
// Confirm that we've found a substantial region.
EXPECT_GE(region.MaximumVolumeInscribedEllipsoid().Volume(), 2.0);

EXPECT_TRUE(region.PointInSet(Vector2d{.4, 0.0}));
EXPECT_FALSE(region.PointInSet(Vector2d{.5, 0.0}));
EXPECT_TRUE(region.PointInSet(Vector2d{.3, .3}));
EXPECT_FALSE(region.PointInSet(Vector2d{.4, .3}));
EXPECT_TRUE(region.PointInSet(Vector2d{-.4, 0.0}));
EXPECT_FALSE(region.PointInSet(Vector2d{-.5, 0.0}));
EXPECT_TRUE(region.PointInSet(Vector2d{-.3, -.3}));
EXPECT_FALSE(region.PointInSet(Vector2d{-.4, -.3}));
}

} // namespace
Expand Down

0 comments on commit d6908d5

Please sign in to comment.