Codegen paths from python for the arm and build the graph
We now have a bunch of really cool paths in C++. They look pretty good.
Change-Id: I9590d771cebc64cd520ceed2617aca8a82f78362
diff --git a/y2018/control_loops/superstructure/arm/BUILD b/y2018/control_loops/superstructure/arm/BUILD
index 997d940..af1e03a 100644
--- a/y2018/control_loops/superstructure/arm/BUILD
+++ b/y2018/control_loops/superstructure/arm/BUILD
@@ -74,8 +74,8 @@
],
restricted_to = ["//tools:k8"],
deps = [
- ":demo_path",
":ekf",
+ ":generated_graph",
":trajectory",
"//third_party/eigen",
"//third_party/gflags",
@@ -126,6 +126,7 @@
deps = [
":demo_path",
":ekf",
+ ":generated_graph",
":graph",
":trajectory",
"//aos/common/logging:queue_logging",
@@ -134,3 +135,27 @@
"//y2018/control_loops/superstructure:superstructure_queue",
],
)
+
+genrule(
+ name = "generated_graph_genrule",
+ outs = [
+ "generated_graph.h",
+ "generated_graph.cc",
+ ],
+ cmd = "$(location //y2018/control_loops/python:graph_codegen) $(OUTS)",
+ tools = [
+ "//y2018/control_loops/python:graph_codegen",
+ ],
+)
+
+cc_library(
+ name = "generated_graph",
+ srcs = [
+ "generated_graph.cc",
+ ],
+ hdrs = ["generated_graph.h"],
+ deps = [
+ ":graph",
+ ":trajectory",
+ ],
+)
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index 484202e..d236efe 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -8,6 +8,7 @@
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/arm/demo_path.h"
#include "y2018/control_loops/superstructure/arm/dynamics.h"
+#include "y2018/control_loops/superstructure/arm/generated_graph.h"
namespace y2018 {
namespace control_loops {
@@ -17,40 +18,20 @@
namespace chrono = ::std::chrono;
using ::aos::monotonic_clock;
-SearchGraph::Edge MakeEdge(::std::vector<TrajectoryPair> *trajectories,
- size_t start, size_t end, ::std::unique_ptr<Path> forwards_path,
- ::std::unique_ptr<Path> backwards_path) {
- trajectories->emplace_back(::std::move(forwards_path),
- ::std::move(backwards_path), 0.005);
-
- return SearchGraph::Edge{start, end,
- trajectories->back().forwards.path().length()};
-}
-
-SearchGraph MakeSearchGraph(::std::vector<TrajectoryPair> *trajectories) {
- ::std::vector<SearchGraph::Edge> edges;
-
- // TODO(austin): Add more trajectories here.
- edges.push_back(
- MakeEdge(trajectories, 0, 1, MakeReversedDemoPath(), MakeDemoPath()));
-
- return SearchGraph(2, ::std::move(edges));
-}
-
Arm::Arm()
: proximal_zeroing_estimator_(constants::GetValues().arm_proximal.zeroing),
distal_zeroing_estimator_(constants::GetValues().arm_distal.zeroing),
alpha_unitizer_((::Eigen::Matrix<double, 2, 2>() << 1.0 / kAlpha0Max(),
0.0, 0.0, 1.0 / kAlpha1Max())
.finished()),
- search_graph_(MakeSearchGraph(&trajectories_)),
+ search_graph_(MakeSearchGraph(&trajectories_, alpha_unitizer_, kVMax())),
// Go to the start of the first trajectory.
- follower_(trajectories_[0].forwards.path().Theta(0)) {
- // TODO(austin): Stop talking about indeces in the list and start talking
- // about points/search for the nearest point.
- for (TrajectoryPair &trajectory_pair : trajectories_) {
- trajectory_pair.forwards.OptimizeTrajectory(alpha_unitizer_, kVMax());
- trajectory_pair.backwards.OptimizeTrajectory(alpha_unitizer_, kVMax());
+ follower_(ReadyAboveBoxPoint()) {
+ int i = 0;
+ for (const auto &trajectory : trajectories_) {
+ LOG(INFO, "trajectory length for edge node %d: %f\n", i,
+ trajectory.path().length());
+ ++i;
}
}
@@ -127,6 +108,10 @@
state_ = State::ESTOP;
} else if (unsafe_goal != nullptr) {
if (!follower_.has_path()) {
+ // TODO(austin): Nearest point at the end of Initialize.
+ // So, get a vector of all the points, loop through them, and find the
+ // closest one.
+
// If we don't have a path and are far from the goal, don't update the
// path.
// TODO(austin): Once we get close to our first setpoint, crank the
@@ -165,13 +150,11 @@
// Ok, now we know which edge we are on. Figure out the path and
// trajectory.
const SearchGraph::Edge &next_edge = search_graph_.edges()[min_edge];
- if (next_edge.start == current_node_) {
- follower_.SwitchTrajectory(&trajectories_[min_edge].forwards);
- current_node_ = next_edge.end;
- } else {
- follower_.SwitchTrajectory(&trajectories_[min_edge].backwards);
- current_node_ = next_edge.start;
- }
+ LOG(INFO, "Switching from node %d to %d along edge %d\n",
+ static_cast<int>(current_node_), static_cast<int>(next_edge.end),
+ static_cast<int>(min_edge));
+ follower_.SwitchTrajectory(&trajectories_[min_edge]);
+ current_node_ = next_edge.end;
}
}
break;
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index d794c3e..1b7c1ce 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -14,16 +14,6 @@
namespace superstructure {
namespace arm {
-struct TrajectoryPair {
- TrajectoryPair(::std::unique_ptr<Path> forwards_path,
- ::std::unique_ptr<Path> backwards_path, double step_size)
- : forwards(::std::move(forwards_path), step_size),
- backwards(::std::move(backwards_path), step_size) {}
-
- Trajectory forwards;
- Trajectory backwards;
-};
-
class Arm {
public:
Arm();
@@ -67,7 +57,7 @@
const ::Eigen::Matrix<double, 2, 2> alpha_unitizer_;
- ::std::vector<TrajectoryPair> trajectories_;
+ ::std::vector<Trajectory> trajectories_;
SearchGraph search_graph_;
bool close_enough_for_full_power_ = false;
diff --git a/y2018/control_loops/superstructure/arm/trajectory.cc b/y2018/control_loops/superstructure/arm/trajectory.cc
index f7746cb..e2337f2 100644
--- a/y2018/control_loops/superstructure/arm/trajectory.cc
+++ b/y2018/control_loops/superstructure/arm/trajectory.cc
@@ -32,6 +32,10 @@
return Path(list);
}
+::std::unique_ptr<Path> Path::Reversed(::std::unique_ptr<Path> p) {
+ return ::std::unique_ptr<Path>(new Path(Path::Reversed(*p)));
+}
+
Path::Path(::std::vector<::std::array<double, 6>> list) {
distances_.reserve(list.size());
::Eigen::Matrix<double, 2, 1> last_theta;
diff --git a/y2018/control_loops/superstructure/arm/trajectory.h b/y2018/control_loops/superstructure/arm/trajectory.h
index d9e918d..38c2b7a 100644
--- a/y2018/control_loops/superstructure/arm/trajectory.h
+++ b/y2018/control_loops/superstructure/arm/trajectory.h
@@ -25,6 +25,7 @@
// doesn't want that.
Path(::std::vector<::std::array<double, 6>> list);
+ static ::std::unique_ptr<Path> Reversed(::std::unique_ptr<Path> p);
static Path Reversed(const Path &p);
// Returns the length of the path in radians.
diff --git a/y2018/control_loops/superstructure/arm/trajectory_plot.cc b/y2018/control_loops/superstructure/arm/trajectory_plot.cc
index 47e6fb7..b1e089c 100644
--- a/y2018/control_loops/superstructure/arm/trajectory_plot.cc
+++ b/y2018/control_loops/superstructure/arm/trajectory_plot.cc
@@ -2,9 +2,9 @@
#include "third_party/gflags/include/gflags/gflags.h"
#include "third_party/matplotlib-cpp/matplotlibcpp.h"
-#include "y2018/control_loops/superstructure/arm/demo_path.h"
#include "y2018/control_loops/superstructure/arm/dynamics.h"
#include "y2018/control_loops/superstructure/arm/ekf.h"
+#include "y2018/control_loops/superstructure/arm/generated_graph.h"
DEFINE_bool(forwards, true, "If true, run the forwards simulation.");
DEFINE_bool(plot, true, "If true, plot");
@@ -16,8 +16,10 @@
namespace arm {
void Main() {
- Trajectory trajectory(
- FLAGS_forwards ? MakeDemoPath() : MakeReversedDemoPath(), 0.001);
+ Trajectory trajectory(FLAGS_forwards
+ ? MakeNeutralToFrontHighPath()
+ : Path::Reversed(MakeNeutralToFrontHighPath()),
+ 0.001);
constexpr double kAlpha0Max = 30.0;
constexpr double kAlpha1Max = 50.0;
@@ -39,6 +41,15 @@
::std::vector<double> alpha0_array;
::std::vector<double> alpha1_array;
+ ::std::vector<double> integrated_distance;
+ ::std::vector<double> integrated_theta0_array;
+ ::std::vector<double> integrated_theta1_array;
+ ::std::vector<double> integrated_omega0_array;
+ ::std::vector<double> integrated_omega1_array;
+
+ ::Eigen::Matrix<double, 2, 1> integrated_theta = trajectory.path().Theta(0);
+ ::Eigen::Matrix<double, 2, 1> integrated_omega = trajectory.path().Omega(0);
+
for (const double d : distance_array) {
const ::Eigen::Matrix<double, 2, 1> theta = trajectory.path().Theta(d);
const ::Eigen::Matrix<double, 2, 1> omega = trajectory.path().Omega(d);
@@ -51,6 +62,19 @@
alpha1_array.push_back(alpha(1, 0));
}
+ const double dd = trajectory.path().length() / 1000.0;
+ for (double d = 0; d <= trajectory.path().length(); d += dd) {
+ integrated_distance.push_back(d);
+ integrated_omega0_array.push_back(integrated_omega(0));
+ integrated_omega1_array.push_back(integrated_omega(1));
+ integrated_theta0_array.push_back(integrated_theta(0));
+ integrated_theta1_array.push_back(integrated_theta(1));
+
+ const ::Eigen::Matrix<double, 2, 1> alpha = trajectory.path().Alpha(d);
+ integrated_theta += integrated_omega * dd;
+ integrated_omega += alpha * dd;
+ }
+
// Next step: see what U is as a function of distance.
::std::vector<double> Uff0_distance_array_curvature;
::std::vector<double> Uff1_distance_array_curvature;
@@ -235,6 +259,15 @@
matplotlibcpp::plot(distance_array, omega1_array, {{"label", "omega1"}});
matplotlibcpp::plot(distance_array, alpha0_array, {{"label", "alpha0"}});
matplotlibcpp::plot(distance_array, alpha1_array, {{"label", "alpha1"}});
+
+ matplotlibcpp::plot(integrated_distance, integrated_theta0_array,
+ {{"label", "itheta0"}});
+ matplotlibcpp::plot(integrated_distance, integrated_theta1_array,
+ {{"label", "itheta1"}});
+ matplotlibcpp::plot(integrated_distance, integrated_omega0_array,
+ {{"label", "iomega0"}});
+ matplotlibcpp::plot(integrated_distance, integrated_omega1_array,
+ {{"label", "iomega1"}});
matplotlibcpp::legend();
matplotlibcpp::figure();