Skip to content

Commit

Permalink
fix line search
Browse files Browse the repository at this point in the history
  • Loading branch information
@mayataka
mayataka committed Oct 30, 2022
1 parent 1baaf57 commit d26019a
Show file tree
Hide file tree
Showing 5 changed files with 42 additions and 66 deletions.
6 changes: 3 additions & 3 deletions src/line_search/line_search.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ double LineSearch::computeStepSize(
else {
throw std::runtime_error("[LineSearch]: Invalid LineSearchMethod");
}
return std::max(primal_step_size, settings_.min_step_size);
return primal_step_size;
}


Expand Down Expand Up @@ -80,7 +80,7 @@ double LineSearch::lineSearchFilterMethod(
}
primal_step_size *= settings_.step_size_reduction_rate;
}
return std::max(primal_step_size, settings_.min_step_size);
return primal_step_size;
}


Expand Down Expand Up @@ -114,7 +114,7 @@ double LineSearch::meritBacktrackingLineSearch(
}
primal_step_size *= settings_.step_size_reduction_rate;
}
return std::max(primal_step_size, settings_.min_step_size);
return primal_step_size;
}


Expand Down
4 changes: 2 additions & 2 deletions src/line_search/unconstr_line_search.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ double UnconstrLineSearch::computeStepSize(
}
primal_step_size *= settings_.step_size_reduction_rate;
}
return std::max(primal_step_size, settings_.min_step_size);
return primal_step_size;
}


Expand Down Expand Up @@ -92,7 +92,7 @@ double UnconstrLineSearch::computeStepSize(
}
primal_step_size *= settings_.step_size_reduction_rate;
}
return std::max(primal_step_size, settings_.min_step_size);
return primal_step_size;
}

} // namespace robotoc
2 changes: 1 addition & 1 deletion test/line_search/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -1,3 +1,3 @@
add_robotoc_test(line_search_filter_test)
# add_robotoc_test(line_search_test)
add_robotoc_test(line_search_test)
add_robotoc_test(unconstr_line_search_test)
60 changes: 17 additions & 43 deletions test/line_search/line_search_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
#include "contact_sequence_factory.hpp"
#include "solution_factory.hpp"
#include "direction_factory.hpp"
#include "kkt_factory.hpp"
#include "cost_factory.hpp"
#include "constraints_factory.hpp"

Expand All @@ -34,49 +35,19 @@ class LineSearchTest : public ::testing::TestWithParam<LineSearchSettings> {
t = std::abs(Eigen::VectorXd::Random(1)[0]);
dt = T / N;
step_size_reduction_rate = 0.75;
min_step_size = 0.05;
}

virtual void TearDown() {
}

Solution createSolution(const Robot& robot) const;
Solution createSolution(const Robot& robot,
const std::shared_ptr<ContactSequence>& contact_sequence) const;
Direction createDirection(const Robot& robot) const;
Direction createDirection(const Robot& robot,
const std::shared_ptr<ContactSequence>& contact_sequence) const;
std::shared_ptr<ContactSequence> createContactSequence(const Robot& robot) const;

void test(const Robot& robot, const LineSearchSettings& settings) const;

int N, max_num_impact, nthreads;
double T, t, dt, step_size_reduction_rate, min_step_size;
double T, t, dt, step_size_reduction_rate;
};


Solution LineSearchTest::createSolution(const Robot& robot) const {
return testhelper::CreateSolution(robot, N, max_num_impact);
}


Solution LineSearchTest::createSolution(const Robot& robot,
const std::shared_ptr<ContactSequence>& contact_sequence) const {
return testhelper::CreateSolution(robot, contact_sequence, T, N, max_num_impact, t);
}


Direction LineSearchTest::createDirection(const Robot& robot) const {
return testhelper::CreateDirection(robot, N, max_num_impact);
}


Direction LineSearchTest::createDirection(const Robot& robot,
const std::shared_ptr<ContactSequence>& contact_sequence) const {
return testhelper::CreateDirection(robot, contact_sequence, T, N, max_num_impact, t);
}


std::shared_ptr<ContactSequence> LineSearchTest::createContactSequence(const Robot& robot) const {
return testhelper::CreateContactSequence(robot, N, max_num_impact, t, 3*dt);
}
Expand All @@ -86,12 +57,11 @@ void LineSearchTest::test(const Robot& robot, const LineSearchSettings& settings
auto cost = testhelper::CreateCost(robot);
auto constraints = testhelper::CreateConstraints(robot);
const auto contact_sequence = createContactSequence(robot);
auto kkt_residual = KKTResidual(robot, N, max_num_impact);
const auto s = createSolution(robot, contact_sequence);
const auto d = createDirection(robot, contact_sequence);
TimeDiscretization time_discretization(T, N);
time_discretization.discretize(contact_sequence, t);
const Eigen::VectorXd q = robot.generateFeasibleConfiguration();
const Eigen::VectorXd v = Eigen::VectorXd::Random(robot.dimv());
auto kkt_residual_ref = kkt_residual;
auto kkt_residual = testhelper::CreateKKTResidual(robot, contact_sequence, time_discretization);
std::vector<Robot, Eigen::aligned_allocator<Robot>> robots(nthreads, robot);
OCP ocp;
ocp.robot = robot;
Expand All @@ -101,15 +71,19 @@ void LineSearchTest::test(const Robot& robot, const LineSearchSettings& settings
ocp.N = N;
ocp.T = T;
DirectMultipleShooting dms(ocp, nthreads);
dms.initConstraints(ocp, robots, contact_sequence, s);
LineSearch line_search(ocp, nthreads, settings);
const double max_primal_step_size = min_step_size + std::abs(Eigen::VectorXd::Random(1)[0]) * (1-min_step_size);
const double step_size = line_search.computeStepSize(ocp, robots, contact_sequence, q, v, s, d, max_primal_step_size);
const auto s = testhelper::CreateSolution(robot, contact_sequence, time_discretization);
const auto d = testhelper::CreateDirection(robot, contact_sequence, time_discretization);
dms.initConstraints(robots, time_discretization, s);
dms.evalOCP(robots, time_discretization, q, v, s, kkt_residual);
LineSearch line_search(ocp, settings);
const double max_primal_step_size = settings.min_step_size + std::abs(Eigen::VectorXd::Random(1)[0]) * (1-settings.min_step_size);
const double step_size = line_search.computeStepSize(dms, robots, time_discretization,
q, v, s, d, max_primal_step_size);
EXPECT_TRUE(step_size <= max_primal_step_size);
EXPECT_TRUE(step_size >= min_step_size);
const double very_small_max_primal_step_size = min_step_size * std::abs(Eigen::VectorXd::Random(1)[0]);
EXPECT_DOUBLE_EQ(line_search.computeStepSize(ocp, robots, contact_sequence, q, v, s, d, very_small_max_primal_step_size),
min_step_size);
EXPECT_TRUE(step_size > 0.0);
const double very_small_max_primal_step_size = settings.min_step_size * std::abs(Eigen::VectorXd::Random(1)[0]);
EXPECT_DOUBLE_EQ(line_search.computeStepSize(dms, robots, time_discretization, q, v, s, d, very_small_max_primal_step_size),
very_small_max_primal_step_size);
EXPECT_NO_THROW(
std::cout << settings << std::endl;
);
Expand Down
36 changes: 19 additions & 17 deletions test/line_search/unconstr_line_search_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,8 +29,6 @@ class UnconstrLineSearchTest : public ::testing::Test {
T = 1;
dt = T / N;
t = std::abs(Eigen::VectorXd::Random(1)[0]);
step_size_reduction_rate = 0.75;
min_step_size = 0.05;
time_discretization.resize(N+1);
for (int i=0; i<=N; ++i) {
time_discretization[i].t = t + dt * i;
Expand All @@ -49,7 +47,7 @@ class UnconstrLineSearchTest : public ::testing::Test {

Robot robot;
int dimv, N, nthreads;
double T, dt, t, step_size_reduction_rate, min_step_size;
double T, dt, t;
std::vector<GridInfo> time_discretization;
};

Expand All @@ -61,20 +59,22 @@ TEST_F(UnconstrLineSearchTest, UnconstrOCP) {
const auto d = testhelper::CreateDirection(robot, N);
const Eigen::VectorXd q = robot.generateFeasibleConfiguration();
const Eigen::VectorXd v = Eigen::VectorXd::Random(robot.dimv());
KKTResidual kkt_residual(N+1, SplitKKTResidual(robot));
aligned_vector<Robot> robots(nthreads, robot);
OCP ocp(robot, cost, constraints, T, N);
UnconstrDirectMultipleShooting dms(ocp, nthreads);
dms.initConstraints(robots, time_discretization, s);
UnconstrLineSearch line_search(ocp);
const double max_primal_step_size = min_step_size + std::abs(Eigen::VectorXd::Random(1)[0]) * (1-min_step_size);
dms.evalOCP(robots, time_discretization, q, v, s, kkt_residual);
LineSearchSettings settings;
UnconstrLineSearch line_search(ocp, settings);
const double max_primal_step_size = settings.min_step_size + std::abs(Eigen::VectorXd::Random(1)[0]) * (1-settings.min_step_size);
const double step_size = line_search.computeStepSize(dms, robots, time_discretization,
q, v, s, d, max_primal_step_size);
EXPECT_TRUE(step_size <= max_primal_step_size);
EXPECT_TRUE(step_size >= min_step_size);
const double very_small_max_primal_step_size = min_step_size * std::abs(Eigen::VectorXd::Random(1)[0]);
const double step_size2 = line_search.computeStepSize(dms, robots, time_discretization,
q, v, s, d, max_primal_step_size);
EXPECT_DOUBLE_EQ(step_size2, min_step_size);
EXPECT_TRUE(step_size > 0.0);
const double very_small_max_primal_step_size = settings.min_step_size * std::abs(Eigen::VectorXd::Random(1)[0]);
EXPECT_DOUBLE_EQ(line_search.computeStepSize(dms, robots, time_discretization, q, v, s, d, very_small_max_primal_step_size),
very_small_max_primal_step_size);
}


Expand All @@ -85,20 +85,22 @@ TEST_F(UnconstrLineSearchTest, UnconstrParNMPC) {
const auto d = testhelper::CreateDirection(robot, N);
const Eigen::VectorXd q = robot.generateFeasibleConfiguration();
const Eigen::VectorXd v = Eigen::VectorXd::Random(robot.dimv());
KKTResidual kkt_residual(N+1, SplitKKTResidual(robot));
aligned_vector<Robot> robots(nthreads, robot);
OCP ocp(robot, cost, constraints, T, N);
UnconstrBackwardCorrection backward_correction(ocp, nthreads);
backward_correction.initConstraints(robots, time_discretization, s);
UnconstrLineSearch line_search(ocp);
const double max_primal_step_size = min_step_size + std::abs(Eigen::VectorXd::Random(1)[0]) * (1-min_step_size);
backward_correction.evalOCP(robots, time_discretization, q, v, s, kkt_residual);
LineSearchSettings settings;
UnconstrLineSearch line_search(ocp, settings);
const double max_primal_step_size = settings.min_step_size + std::abs(Eigen::VectorXd::Random(1)[0]) * (1-settings.min_step_size);
const double step_size = line_search.computeStepSize(backward_correction, robots, time_discretization,
q, v, s, d, max_primal_step_size);
EXPECT_TRUE(step_size <= max_primal_step_size);
EXPECT_TRUE(step_size >= min_step_size);
const double very_small_max_primal_step_size = min_step_size * std::abs(Eigen::VectorXd::Random(1)[0]);
const double step_size2 = line_search.computeStepSize(backward_correction, robots, time_discretization,
q, v, s, d, max_primal_step_size);
EXPECT_DOUBLE_EQ(step_size2, min_step_size);
EXPECT_TRUE(step_size > 0.0);
const double very_small_max_primal_step_size = settings.min_step_size * std::abs(Eigen::VectorXd::Random(1)[0]);
EXPECT_DOUBLE_EQ(line_search.computeStepSize(backward_correction, robots, time_discretization, q, v, s, d, very_small_max_primal_step_size),
very_small_max_primal_step_size);
}

} // namespace robotoc
Expand Down

0 comments on commit d26019a

Please sign in to comment.