Per path max voltage.
Change-Id: Iee719699ec392192330c92c9eaf9596fa0d9c250
diff --git a/y2018/control_loops/python/graph_codegen.py b/y2018/control_loops/python/graph_codegen.py
index d5dbe12..5559f7c 100644
--- a/y2018/control_loops/python/graph_codegen.py
+++ b/y2018/control_loops/python/graph_codegen.py
@@ -14,13 +14,19 @@
def add_edge(cc_file, name, segment, index, reverse):
cc_file.append(" // Adding edge %d" % index)
+ vmax = "vmax"
+ if segment.vmax:
+ vmax = "::std::min(vmax, %f)" % segment.vmax
+ cc_file.append( " trajectories->emplace_back(%s," % (vmax))
+ cc_file.append( " alpha_unitizer,")
if reverse:
cc_file.append(
- " trajectories->emplace_back(Path::Reversed(%s()), 0.005);" %
- (path_function_name(str(name))))
+ " Trajectory(Path::Reversed(%s()), 0.005));"
+ % (path_function_name(str(name))))
else:
- cc_file.append(" trajectories->emplace_back(%s(), 0.005);" %
- (path_function_name(str(name))))
+ cc_file.append(
+ " Trajectory(%s(), 0.005));"
+ % (path_function_name(str(name))))
start_index = None
end_index = None
@@ -37,11 +43,13 @@
(index_function_name(start_index),
index_function_name(end_index)))
cc_file.append(
- " (trajectories->back().path().length() + 0.2)});")
+ " (trajectories->back().trajectory.path().length() + 1.0)});")
# TODO(austin): Allow different vmaxes for different paths.
cc_file.append(
- " trajectories->back().OptimizeTrajectory(alpha_unitizer, vmax);")
+ " trajectories->back().trajectory.OptimizeTrajectory(")
+ cc_file.append(" trajectories->back().alpha_unitizer,")
+ cc_file.append(" trajectories->back().vmax);")
cc_file.append("")
@@ -81,6 +89,22 @@
h_file.append("namespace superstructure {")
h_file.append("namespace arm {")
+ h_file.append("")
+ h_file.append("struct TrajectoryAndParams {")
+ h_file.append(" TrajectoryAndParams(double new_vmax,")
+ h_file.append(
+ " const ::Eigen::Matrix<double, 2, 2> &new_alpha_unitizer,"
+ )
+ h_file.append(" Trajectory &&new_trajectory)")
+ h_file.append(" : vmax(new_vmax),")
+ h_file.append(" alpha_unitizer(new_alpha_unitizer),")
+ h_file.append(" trajectory(::std::move(new_trajectory)) {}")
+ h_file.append(" double vmax;")
+ h_file.append(" ::Eigen::Matrix<double, 2, 2> alpha_unitizer;")
+ h_file.append(" Trajectory trajectory;")
+ h_file.append("};")
+ h_file.append("")
+
# Now dump out the vertices and associated constexpr vertex name functions.
for index, point in enumerate(graph_generate.points):
h_file.append("")
@@ -130,12 +154,12 @@
h_file.append("")
h_file.append("// Builds a search graph.")
h_file.append("SearchGraph MakeSearchGraph("
- "::std::vector<Trajectory> *trajectories,")
+ "::std::vector<TrajectoryAndParams> *trajectories,")
h_file.append(" "
"const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer,")
h_file.append(" double vmax);")
cc_file.append("SearchGraph MakeSearchGraph("
- "::std::vector<Trajectory> *trajectories,")
+ "::std::vector<TrajectoryAndParams> *trajectories,")
cc_file.append(" "
"const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer,")
cc_file.append(" " "double vmax) {")
diff --git a/y2018/control_loops/python/graph_generate.py b/y2018/control_loops/python/graph_generate.py
index aa91137..7ffebf6 100644
--- a/y2018/control_loops/python/graph_generate.py
+++ b/y2018/control_loops/python/graph_generate.py
@@ -195,7 +195,7 @@
# Segment in angle space.
class AngleSegment:
- def __init__(self, start, end, name=None):
+ def __init__(self, start, end, name=None, alpha_unitizer=None, vmax=None):
"""Creates an angle segment.
Args:
@@ -207,6 +207,8 @@
self.start = start
self.end = end
self.name = name
+ self.alpha_unitizer = alpha_unitizer
+ self.vmax = vmax
def __repr__(self):
return "AngleSegment(%s, %s)" % (repr(self.start), repr(self.end))
@@ -245,7 +247,7 @@
class XYSegment:
"""Straight line in XY space."""
- def __init__(self, start, end, name=None):
+ def __init__(self, start, end, name=None, alpha_unitizer=None, vmax=None):
"""Creates an XY segment.
Args:
@@ -257,6 +259,8 @@
self.start = start
self.end = end
self.name = name
+ self.alpha_unitizer = alpha_unitizer
+ self.vmax = vmax
def __repr__(self):
return "XYSegment(%s, %s)" % (repr(self.start), repr(self.end))
@@ -329,21 +333,30 @@
class SplineSegment:
- def __init__(self, start, control1, control2, end, name=None):
+ def __init__(self,
+ start,
+ control1,
+ control2,
+ end,
+ name=None,
+ alpha_unitizer=None,
+ vmax=None):
self.start = start
self.control1 = control1
self.control2 = control2
self.end = end
self.name = name
+ self.alpha_unitizer = alpha_unitizer
+ self.vmax = vmax
def __repr__(self):
- return "XYSegment(%s, %s, &s, %s)" % (repr(self.start),
- repr(self.control1),
- repr(self.control2),
- repr(self.end))
+ return "SplineSegment(%s, %s, %s, %s)" % (repr(self.start),
+ repr(self.control1),
+ repr(self.control2),
+ repr(self.end))
def DrawTo(self, cr, theta_version):
- if (theta_version):
+ if theta_version:
c_i_select = get_circular_index(self.start)
start = get_xy(self.start)
control1 = get_xy(self.control1)
@@ -356,17 +369,27 @@
c_i_select)
for alpha in subdivide_spline(start, control1, control2, end)
])
+ cr.move_to(self.start[0] + theta_end_circle_size, self.start[1])
+ cr.arc(self.start[0], self.start[1], theta_end_circle_size, 0,
+ 2.0 * numpy.pi)
+ cr.move_to(self.end[0] + theta_end_circle_size, self.end[1])
+ cr.arc(self.end[0], self.end[1], theta_end_circle_size, 0,
+ 2.0 * numpy.pi)
else:
start = get_xy(self.start)
control1 = get_xy(self.control1)
control2 = get_xy(self.control2)
end = get_xy(self.end)
- #cr.move_to(start[0], start[1])
+
draw_lines(cr, [
spline_eval(start, control1, control2, end, alpha)
for alpha in subdivide_spline(start, control1, control2, end)
])
- # cr.spline_to(control1[0], control1[1], control2[0], control2[1], end[0], end[1])
+
+ cr.move_to(self.start[0] + xy_end_circle_size, start[1])
+ cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+ cr.move_to(end[0] + xy_end_circle_size, end[1])
+ cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
def ToThetaPoints(self):
t1, t2 = self.start
@@ -386,6 +409,76 @@
]
+def get_derivs(t_prev, t, t_next):
+ c, a, b = t_prev, t, t_next
+ d1 = normalize(b - a)
+ d2 = normalize(c - a)
+ accel = (d1 + d2) / numpy.linalg.norm(a - b)
+ return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
+
+
+class ThetaSplineSegment:
+ def __init__(self,
+ start,
+ control1,
+ control2,
+ end,
+ name=None,
+ alpha_unitizer=None,
+ vmax=None):
+ self.start = start
+ self.control1 = control1
+ self.control2 = control2
+ self.end = end
+ self.name = name
+ self.alpha_unitizer = alpha_unitizer
+ self.vmax = vmax
+
+ def __repr__(self):
+ return "ThetaSplineSegment(%s, %s, &s, %s)" % (repr(self.start),
+ repr(self.control1),
+ repr(self.control2),
+ repr(self.end))
+
+ def DrawTo(self, cr, theta_version):
+ if (theta_version):
+ draw_lines(cr, [
+ spline_eval(self.start, self.control1, self.control2, self.end,
+ alpha)
+ for alpha in subdivide_spline(self.start, self.control1,
+ self.control2, self.end)
+ ])
+ else:
+ start = get_xy(self.start)
+ end = get_xy(self.end)
+
+ draw_lines(cr, [
+ get_xy(
+ spline_eval(self.start, self.control1, self.control2,
+ self.end, alpha))
+ for alpha in subdivide_spline(self.start, self.control1,
+ self.control2, self.end)
+ ])
+
+ cr.move_to(start[0] + xy_end_circle_size, start[1])
+ cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+ cr.move_to(end[0] + xy_end_circle_size, end[1])
+ cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+
+ def ToThetaPoints(self):
+ return [
+ get_derivs(
+ spline_eval(self.start, self.control1, self.control2, self.end,
+ alpha - 0.00001),
+ spline_eval(self.start, self.control1, self.control2, self.end,
+ alpha),
+ spline_eval(self.start, self.control1, self.control2, self.end,
+ alpha + 0.00001))
+ for alpha in subdivide_spline(self.start, self.control1,
+ self.control2, self.end)
+ ]
+
+
tall_box_x = 0.411
tall_box_y = 0.125
@@ -486,11 +579,11 @@
# We need to define critical points so we can create paths connecting them.
# TODO(austin): Attach velocities to the slow ones.
named_segments = [
- XYSegment(ready_above_box, tall_box_grab, "ReadyToTallBox"),
- XYSegment(ready_above_box, short_box_grab, "ReadyToShortBox"),
- XYSegment(tall_box_grab, short_box_grab, "TallToShortBox"),
SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
ready_above_box, "ReadyToNeutral"),
+ XYSegment(ready_above_box, tall_box_grab, "ReadyToTallBox", vmax=6.0),
+ XYSegment(ready_above_box, short_box_grab, "ReadyToShortBox", vmax=6.0),
+ XYSegment(tall_box_grab, short_box_grab, "TallToShortBox", vmax=6.0),
SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
tall_box_grab, "TallToNeutral"),
SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index 4912359..d3997ca 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -31,7 +31,7 @@
int i = 0;
for (const auto &trajectory : trajectories_) {
LOG(INFO, "trajectory length for edge node %d: %f\n", i,
- trajectory.path().length());
+ trajectory.trajectory.path().length());
++i;
}
}
@@ -330,7 +330,8 @@
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]);
+ vmax_ = trajectories_[min_edge].vmax;
+ follower_.SwitchTrajectory(&trajectories_[min_edge].trajectory);
current_node_ = next_edge.end;
}
}
@@ -340,7 +341,7 @@
close_enough_for_full_power_
? kOperatingVoltage()
: (state_ == State::GOTO_PATH ? kGotoPathVMax() : kPathlessVMax());
- follower_.Update(arm_ekf_.X_hat(), disable, kDt(), kVMax(),
+ follower_.Update(arm_ekf_.X_hat(), disable, kDt(), vmax_,
max_operating_voltage);
LOG(INFO, "Max voltage: %f\n", max_operating_voltage);
status->goal_theta0 = follower_.theta(0);
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 840ce53..d86b95b 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -5,6 +5,7 @@
#include "y2018/constants.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"
#include "y2018/control_loops/superstructure/arm/graph.h"
#include "y2018/control_loops/superstructure/arm/trajectory.h"
#include "y2018/control_loops/superstructure/superstructure.q.h"
@@ -23,15 +24,15 @@
// The operating voltage.
static constexpr double kOperatingVoltage() {
- return kGrannyMode() ? 4.0 : 12.0;
+ return kGrannyMode() ? 5.0 : 12.0;
}
static constexpr double kDt() { return 0.00505; }
- static constexpr double kAlpha0Max() { return kGrannyMode() ? 10.0 : 10.0; }
- static constexpr double kAlpha1Max() { return kGrannyMode() ? 10.0 : 10.0; }
+ static constexpr double kAlpha0Max() { return kGrannyMode() ? 5.0 : 15.0; }
+ static constexpr double kAlpha1Max() { return kGrannyMode() ? 5.0 : 15.0; }
static constexpr double kVMax() { return kGrannyMode() ? 5.0 : 11.5; }
static constexpr double kPathlessVMax() { return 5.0; }
- static constexpr double kGotoPathVMax() { return 12.0; }
+ static constexpr double kGotoPathVMax() { return 6.0; }
void Iterate(const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
const control_loops::ArmPosition *position,
@@ -96,7 +97,9 @@
const ::Eigen::Matrix<double, 2, 2> alpha_unitizer_;
- ::std::vector<Trajectory> trajectories_;
+ double vmax_ = kVMax();
+
+ ::std::vector<TrajectoryAndParams> trajectories_;
SearchGraph search_graph_;
bool close_enough_for_full_power_ = false;