tuned and redid the capping on the claw to prioritize separation error
diff --git a/aos/controls/polytope.h b/aos/controls/polytope.h
index a873722..c3f66cc 100644
--- a/aos/controls/polytope.h
+++ b/aos/controls/polytope.h
@@ -42,10 +42,10 @@
int num_constraints() const { return k_.rows(); }
// Returns true if the point is inside the polytope.
- bool IsInside(Eigen::Matrix<double, number_of_dimensions, 1> point);
+ bool IsInside(Eigen::Matrix<double, number_of_dimensions, 1> point) const;
// Returns the list of vertices inside the polytope.
- Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> Vertices();
+ Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> Vertices() const;
private:
Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions> H_;
@@ -54,7 +54,7 @@
template <int number_of_dimensions>
bool HPolytope<number_of_dimensions>::IsInside(
- Eigen::Matrix<double, number_of_dimensions, 1> point) {
+ Eigen::Matrix<double, number_of_dimensions, 1> point) const {
auto ev = H_ * point;
for (int i = 0; i < num_constraints(); ++i) {
if (ev(i, 0) > k_(i, 0)) {
@@ -66,7 +66,7 @@
template <int number_of_dimensions>
Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic>
- HPolytope<number_of_dimensions>::Vertices() {
+ HPolytope<number_of_dimensions>::Vertices() const {
dd_MatrixPtr matrix = dd_CreateMatrix(num_constraints(), ndim() + 1);
// Copy the data over. TODO(aschuh): Is there a better way? I hate copying...
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 58b2ca0..cbd4d2a 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -5,6 +5,7 @@
#include "aos/common/control_loop/control_loops.q.h"
#include "aos/common/logging/logging.h"
#include "aos/common/logging/queue_logging.h"
+#include "aos/common/logging/matrix_logging.h"
#include "frc971/constants.h"
#include "frc971/control_loops/claw/claw_motor_plant.h"
@@ -44,6 +45,17 @@
namespace frc971 {
namespace control_loops {
+namespace {
+
+template <typename T> int sign(T val) {
+ if (val > T(0)) {
+ return 1;
+ } else {
+ return -1;
+ }
+}
+
+} // namespace
static const double kZeroingVoltage = 4.0;
static const double kMaxVoltage = 12.0;
@@ -57,7 +69,14 @@
0, 1,
0, -1).finished(),
(Eigen::Matrix<double, 4, 1>() << kMaxVoltage, kMaxVoltage,
- kMaxVoltage, kMaxVoltage).finished()) {
+ kMaxVoltage, kMaxVoltage).finished()),
+ U_Poly_zeroing_((Eigen::Matrix<double, 4, 2>() << 1, 0,
+ -1, 0,
+ 0, 1,
+ 0, -1).finished(),
+ (Eigen::Matrix<double, 4, 1>() <<
+ kZeroingVoltage, kZeroingVoltage,
+ kZeroingVoltage, kZeroingVoltage).finished()) {
::aos::controls::HPolytope<0>::Init();
}
@@ -79,6 +98,7 @@
double max_voltage = is_zeroing_ ? kZeroingVoltage : kMaxVoltage;
if (::std::abs(u_bottom) > max_voltage or ::std::abs(u_top) > max_voltage) {
+ LOG_MATRIX(DEBUG, "U at start", U);
// H * U <= k
// U = UPos + UVel
// H * (UPos + UVel) <= k
@@ -100,22 +120,59 @@
position_error << error(0, 0), error(1, 0);
Eigen::Matrix<double, 2, 1> velocity_error;
velocity_error << error(2, 0), error(3, 0);
+ LOG_MATRIX(DEBUG, "error", error);
- Eigen::Matrix<double, 4, 1> pos_poly_k =
- U_Poly_.k() - U_Poly_.H() * velocity_K * velocity_error;
- Eigen::Matrix<double, 4, 2> pos_poly_H = U_Poly_.H() * position_K;
- ::aos::controls::HPolytope<2> pos_poly(pos_poly_H, pos_poly_k);
+ const auto &poly = is_zeroing_ ? U_Poly_zeroing_ : U_Poly_;
+ const Eigen::Matrix<double, 4, 2> pos_poly_H = poly.H() * position_K;
+ const Eigen::Matrix<double, 4, 1> pos_poly_k =
+ poly.k() - poly.H() * velocity_K * velocity_error;
+ const ::aos::controls::HPolytope<2> pos_poly(pos_poly_H, pos_poly_k);
+ Eigen::Matrix<double, 2, 1> adjusted_pos_error;
+ {
+ const auto &P = position_error;
+ Eigen::Matrix<double, 1, 2> L45;
+ L45 << sign(P(1, 0)), -sign(P(0, 0));
+ const double w45 = 0;
- Eigen::Matrix<double, 2, 1> adjusted_pos_error = CoerceGoal(
- pos_poly, (Eigen::Matrix<double, 1, 2>() << position_error(1, 0),
- -position_error(0, 0)).finished(),
- 0.0, position_error);
+ Eigen::Matrix<double, 1, 2> LH;
+ if (::std::abs(P(0, 0)) > ::std::abs(P(1, 0))) {
+ LH << 0, 1;
+ } else {
+ LH << 1, 0;
+ }
+ const double wh = LH.dot(P);
- LOG(DEBUG, "Capping U is now %f %f\n", U(0, 0), U(1, 0));
+ Eigen::Matrix<double, 2, 2> standard;
+ standard << L45, LH;
+ Eigen::Matrix<double, 2, 1> W;
+ W << w45, wh;
+ const Eigen::Matrix<double, 2, 1> intersection = standard.inverse() * W;
+
+ bool is_inside_h;
+ const auto adjusted_pos_error_h =
+ DoCoerceGoal(pos_poly, LH, wh, position_error, &is_inside_h);
+ const auto adjusted_pos_error_45 =
+ DoCoerceGoal(pos_poly, L45, w45, intersection, nullptr);
+ if (pos_poly.IsInside(intersection)) {
+ adjusted_pos_error = adjusted_pos_error_h;
+ } else {
+ if (is_inside_h) {
+ if (adjusted_pos_error_h.norm() > adjusted_pos_error_45.norm()) {
+ adjusted_pos_error = adjusted_pos_error_h;
+ } else {
+ adjusted_pos_error = adjusted_pos_error_45;
+ }
+ } else {
+ adjusted_pos_error = adjusted_pos_error_45;
+ }
+ }
+ }
+
+ LOG_MATRIX(DEBUG, "adjusted_pos_error", adjusted_pos_error);
U = velocity_K * velocity_error + position_K * adjusted_pos_error;
+ LOG_MATRIX(DEBUG, "U is now", U);
}
-
}
ZeroedStateFeedbackLoop::ZeroedStateFeedbackLoop(const char *name,
@@ -741,13 +798,12 @@
case FINE_TUNE_BOTTOM:
case FINE_TUNE_TOP:
case UNKNOWN_LOCATION: {
- Eigen::Matrix<double, 2, 1> U = claw_.K() * (claw_.R - claw_.X_hat);
- LOG(DEBUG, "Uncapped voltages: Top: %f, Bottom: %f\n", U(1, 0), U(0, 0));
+ LOG(DEBUG, "Uncapped voltages: Top: %f, Bottom: %f\n", claw_.U_uncapped(1, 0), claw_.U_uncapped(0, 0));
if (claw_.uncapped_average_voltage() > values.claw.max_zeroing_voltage) {
- double dx_bot = (U(0, 0) -
+ double dx_bot = (claw_.U_uncapped(0, 0) -
values.claw.max_zeroing_voltage) /
claw_.K(0, 0);
- double dx_top = (U(1, 0) -
+ double dx_top = (claw_.U_uncapped(1, 0) -
values.claw.max_zeroing_voltage) /
claw_.K(0, 0);
double dx = ::std::max(dx_top, dx_bot);
@@ -755,7 +811,7 @@
top_claw_goal_ -= dx;
Eigen::Matrix<double, 4, 1> R;
R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0), claw_.R(3, 0);
- U = claw_.K() * (R - claw_.X_hat);
+ claw_.U = claw_.K() * (R - claw_.X_hat);
capped_goal_ = true;
LOG(DEBUG, "Moving the goal by %f to prevent windup."
" Uncapped is %f, max is %f, difference is %f\n",
@@ -765,10 +821,10 @@
values.claw.max_zeroing_voltage));
} else if (claw_.uncapped_average_voltage() <
-values.claw.max_zeroing_voltage) {
- double dx_bot = (U(0, 0) +
+ double dx_bot = (claw_.U_uncapped(0, 0) +
values.claw.max_zeroing_voltage) /
claw_.K(0, 0);
- double dx_top = (U(1, 0) +
+ double dx_top = (claw_.U_uncapped(1, 0) +
values.claw.max_zeroing_voltage) /
claw_.K(0, 0);
double dx = ::std::min(dx_top, dx_bot);
@@ -776,7 +832,7 @@
top_claw_goal_ -= dx;
Eigen::Matrix<double, 4, 1> R;
R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0), claw_.R(3, 0);
- U = claw_.K() * (R - claw_.X_hat);
+ claw_.U = claw_.K() * (R - claw_.X_hat);
capped_goal_ = true;
LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
}
diff --git a/frc971/control_loops/claw/claw.gyp b/frc971/control_loops/claw/claw.gyp
index bdce229..5982378 100644
--- a/frc971/control_loops/claw/claw.gyp
+++ b/frc971/control_loops/claw/claw.gyp
@@ -32,6 +32,7 @@
'<(DEPTH)/aos/build/externals.gyp:libcdd',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
'<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(AOS)/common/logging/logging.gyp:matrix_logging',
],
'export_dependent_settings': [
'claw_loop',
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index dab1dca..6dbc408 100644
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -39,7 +39,7 @@
double uncapped_average_voltage_;
bool is_zeroing_;
- const ::aos::controls::HPolytope<2> U_Poly_;
+ const ::aos::controls::HPolytope<2> U_Poly_, U_Poly_zeroing_;
};
class ClawMotor;
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index 8704ad0..61813bc 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -304,7 +304,7 @@
claw_motor_plant_.GetAbsolutePosition(TOP_CLAW) - bottom;
EXPECT_NEAR(claw_queue_group.goal->bottom_angle, bottom, 1e-4);
EXPECT_NEAR(claw_queue_group.goal->separation_angle, separation, 1e-4);
- EXPECT_TRUE(min_separation_ <= separation);
+ EXPECT_LE(min_separation_, separation);
}
diff --git a/frc971/control_loops/claw/claw_motor_plant.cc b/frc971/control_loops/claw/claw_motor_plant.cc
index 8da2415..28151ad 100644
--- a/frc971/control_loops/claw/claw_motor_plant.cc
+++ b/frc971/control_loops/claw/claw_motor_plant.cc
@@ -9,9 +9,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeClawPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.0, 0.00737284608086, 0.0, 0.0, 1.0, -0.00145272885484, 0.00592011722602, 0.0, 0.0, 0.525184383468, 0.0, 0.0, 0.0, -0.211450629042, 0.313733754426;
+ A << 1.0, 0.0, 0.00909331035298, 0.0, 0.0, 1.0, -6.04514323962e-05, 0.00903285892058, 0.0, 0.0, 0.824315642255, 0.0, 0.0, 0.0, -0.0112975266368, 0.813018115618;
Eigen::Matrix<double, 4, 2> B;
- B << 0.00102145540588, 0.0, -0.00102145540588, 0.00158628631709, 0.184611558069, 0.0, -0.184611558069, 0.26682500835;
+ B << 0.000352527133889, 0.0, -0.000352527133889, 0.000376031064118, 0.0683072794628, 0.0, -0.0683072794628, 0.0726998350615;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 1, 1, 0, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -25,9 +25,9 @@
StateFeedbackController<4, 2, 2> MakeClawController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.48518438347, -2.35513868803e-16, -1.48518438347, 1.27373375443, 34.6171964667, -5.41681898246e-15, -34.6171964667, 14.5766570483;
+ L << 1.72431564225, 1.53329341668e-19, -1.72431564225, 1.71301811562, 65.9456997026, -0, -65.9456997026, 64.4642687194;
Eigen::Matrix<double, 2, 4> K;
- K << 104.272994613, 0.0, 1.72618753001, 0.0, 67.1443817466, 107.935909674, 0.195736876688, 0.983852673373;
+ K << 106.138028875, 0.0, 4.20012492658, 0.0, 99.5038407367, 99.7251230882, 3.77683310096, 3.78980738032;
return StateFeedbackController<4, 2, 2>(L, K, MakeClawPlantCoefficients());
}
diff --git a/frc971/control_loops/claw/claw_motor_plant.h b/frc971/control_loops/claw/claw_motor_plant.h
index 95d0fee..76e3fe7 100644
--- a/frc971/control_loops/claw/claw_motor_plant.h
+++ b/frc971/control_loops/claw/claw_motor_plant.h
@@ -14,7 +14,7 @@
StateFeedbackLoop<4, 2, 2> MakeClawLoop();
-const double kClawMomentOfInertiaRatio = 0.555556;
+const double kClawMomentOfInertiaRatio = 0.933333;
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/coerce_goal.cc b/frc971/control_loops/coerce_goal.cc
index b32b590..de6b757 100644
--- a/frc971/control_loops/coerce_goal.cc
+++ b/frc971/control_loops/coerce_goal.cc
@@ -7,11 +7,13 @@
namespace frc971 {
namespace control_loops {
-Eigen::Matrix<double, 2, 1> CoerceGoal(aos::controls::HPolytope<2> ®ion,
- const Eigen::Matrix<double, 1, 2> &K,
- double w,
- const Eigen::Matrix<double, 2, 1> &R) {
+Eigen::Matrix<double, 2, 1> DoCoerceGoal(const aos::controls::HPolytope<2> ®ion,
+ const Eigen::Matrix<double, 1, 2> &K,
+ double w,
+ const Eigen::Matrix<double, 2, 1> &R,
+ bool *is_inside) {
if (region.IsInside(R)) {
+ if (is_inside) *is_inside = true;
return R;
}
Eigen::Matrix<double, 2, 1> parallel_vector;
@@ -36,6 +38,7 @@
min_distance_sqr = length;
}
}
+ if (is_inside) *is_inside = true;
return closest_point;
} else {
Eigen::Matrix<double, 2, Eigen::Dynamic> region_vertices =
@@ -50,6 +53,7 @@
min_distance = length;
}
}
+ if (is_inside) *is_inside = false;
return (Eigen::Matrix<double, 2, 1>() << region_vertices(0, closest_i),
region_vertices(1, closest_i)).finished();
}
diff --git a/frc971/control_loops/coerce_goal.h b/frc971/control_loops/coerce_goal.h
index 43707b4..59049c5 100644
--- a/frc971/control_loops/coerce_goal.h
+++ b/frc971/control_loops/coerce_goal.h
@@ -8,14 +8,23 @@
namespace frc971 {
namespace control_loops {
+Eigen::Matrix<double, 2, 1> DoCoerceGoal(const aos::controls::HPolytope<2> ®ion,
+ const Eigen::Matrix<double, 1, 2> &K,
+ double w,
+ const Eigen::Matrix<double, 2, 1> &R,
+ bool *is_inside);
+
// Intersects a line with a region, and finds the closest point to R.
// Finds a point that is closest to R inside the region, and on the line
// defined by K X = w. If it is not possible to find a point on the line,
// finds a point that is inside the region and closest to the line.
-Eigen::Matrix<double, 2, 1> CoerceGoal(aos::controls::HPolytope<2> ®ion,
- const Eigen::Matrix<double, 1, 2> &K,
- double w,
- const Eigen::Matrix<double, 2, 1> &R);
+static inline Eigen::Matrix<double, 2, 1>
+ CoerceGoal(const aos::controls::HPolytope<2> ®ion,
+ const Eigen::Matrix<double, 1, 2> &K,
+ double w,
+ const Eigen::Matrix<double, 2, 1> &R) {
+ return DoCoerceGoal(region, K, w, R, nullptr);
+}
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index cb11a8b..9a24e22 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -20,9 +20,8 @@
# Free Current in Amps
self.free_current = 2.7
# Moment of inertia of the claw in kg m^2
- # measured from CAD
- self.J_top = 0.5
- self.J_bottom = 0.9
+ self.J_top = 2.8
+ self.J_bottom = 3.0
# Resistance of the motor
self.R = 12.0 / self.stall_current
@@ -96,8 +95,8 @@
self.A_diff, self.B_diff = controls.c2d(
self.A_diff_cont, self.B_diff_cont, self.dt)
- self.K_bottom = controls.dplace(self.A_bottom, self.B_bottom, [.65, .45])
- self.K_diff = controls.dplace(self.A_diff, self.B_diff, [.60, .28])
+ self.K_bottom = controls.dplace(self.A_bottom, self.B_bottom, [.75 + 0.1j, .75 - 0.1j])
+ self.K_diff = controls.dplace(self.A_diff, self.B_diff, [.75 + 0.1j, .75 - 0.1j])
print "K_diff", self.K_diff
print "K_bottom", self.K_bottom
@@ -145,8 +144,8 @@
print "eigenvalues"
print numpy.linalg.eig(F)[0]
- self.rpl = .02
- self.ipl = 0.004
+ self.rpl = .05
+ self.ipl = 0.010
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
self.rpl + 1j * self.ipl,
self.rpl - 1j * self.ipl,
@@ -301,6 +300,7 @@
claw: claw object containing moments of inertia and U limits.
U: Input matrix to clip as necessary.
"""
+
bottom_u = U[0, 0]
top_u = U[1, 0]
@@ -338,8 +338,48 @@
claw.U_poly.H * position_K,
claw.U_poly.k - claw.U_poly.H * velocity_K * velocity_error)
- adjusted_pos_error = polydrivetrain.CoerceGoal(pos_poly, numpy.matrix([[position_error[1, 0], -position_error[0, 0]]]), 0.0, position_error)
+ P = position_error
+ #K = numpy.matrix([[position_error[1, 0], -position_error[0, 0]]])
+ L45 = numpy.matrix([[numpy.sign(P[1, 0]), -numpy.sign(P[0, 0])]])
+ if L45[0, 1] == 0:
+ L45[0, 1] = 1
+ if L45[0, 0] == 0:
+ L45[0, 0] = 1
+ w45 = numpy.matrix([[0]])
+ if numpy.abs(P[0, 0]) > numpy.abs(P[1, 0]):
+ LH = numpy.matrix([[0, 1]])
+ else:
+ LH = numpy.matrix([[1, 0]])
+ wh = LH * P
+ standard = numpy.concatenate((L45, LH))
+ #print "Standard", standard
+ W = numpy.concatenate((w45, wh))
+ #print "W is", W
+ intersection = numpy.linalg.inv(standard) * W
+ print "intersection point %s" % intersection
+ print "Intersection power is ", velocity_K * velocity_error + position_K * intersection
+ adjusted_pos_error_h, is_inside_h = polydrivetrain.DoCoerceGoal(pos_poly,
+ LH, wh, position_error)
+ adjusted_pos_error_45, is_inside_45 = polydrivetrain.DoCoerceGoal(pos_poly,
+ L45, w45, intersection)
+ if pos_poly.IsInside(intersection):
+ adjusted_pos_error = adjusted_pos_error_h
+ print "horizontal"
+ else:
+ if is_inside_h:
+ if numpy.linalg.norm(adjusted_pos_error_h) > numpy.linalg.norm(adjusted_pos_error_45):
+ adjusted_pos_error = adjusted_pos_error_h
+ else:
+ adjusted_pos_error = adjusted_pos_error_45
+ else:
+ adjusted_pos_error = adjusted_pos_error_45
+ print "45"
+ print velocity_K * velocity_error + position_K * adjusted_pos_error
+ print "45"
+ print adjusted_pos_error
+
+ print "Actual power is ", velocity_K * velocity_error + position_K * adjusted_pos_error
return velocity_K * velocity_error + position_K * adjusted_pos_error
#U = Kpos * poserror + Kvel * velerror
@@ -423,7 +463,7 @@
The tests themselves are not terribly sophisticated; I just test for
whether the goal has been reached and whether the separation goes
- outside of the initial and goal values by more then max_separation_error.
+ outside of the initial and goal values by more than max_separation_error.
Prints out something for a failure of either condition and returns
False if tests fail.
Args:
@@ -498,11 +538,16 @@
R = numpy.matrix([[0.0], [1.0], [0.0], [0.0]])
run_test(claw, initial_X, R)
- # Test changing both separation and position at once..
+ # Test changing both separation and position at once.
initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
R = numpy.matrix([[1.0], [1.0], [0.0], [0.0]])
run_test(claw, initial_X, R)
+ # Test a small separation error and a large position one.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[2.0], [0.05], [0.0], [0.0]])
+ run_test(claw, initial_X, R)
+
# Write the generated constants out to a file.
if len(argv) != 3:
print "Expected .h file name and .cc file name for the claw."
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index f2dfdbe..2b24396 100755
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -28,9 +28,11 @@
Returns:
numpy.matrix (2 x 1), the point.
"""
+ return DoCoerceGoal(region, K, w, R)[0]
+def DoCoerceGoal(region, K, w, R):
if region.IsInside(R):
- return R
+ return (R, True)
perpendicular_vector = K.T / numpy.linalg.norm(K)
parallel_vector = numpy.matrix([[perpendicular_vector[1, 0]],
@@ -79,7 +81,7 @@
min_distance = length
closest_point = point
- return closest_point
+ return (closest_point, True)
else:
# Find the vertex of the space that is closest to the line.
region_vertices = region.Vertices()
@@ -92,7 +94,7 @@
min_distance = length
closest_point = point
- return closest_point
+ return (closest_point, False)
class VelocityDrivetrainModel(control_loop.ControlLoop):