Fixed profile offsetting bug in superstructure.
Change-Id: Ica0f78cfdf16fc9779b478fd8abeb0f86ccbc96d
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index aa43758..3e5cde4 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -97,6 +97,14 @@
const Eigen::Matrix<double, 3, 1> &goal() const { return loop_->R(); }
double goal(int row, int col) const { return loop_->R(row, col); }
+ // Returns the unprofiled goal.
+ const Eigen::Matrix<double, 3, 1> &unprofiled_goal() const {
+ return unprofiled_goal_;
+ }
+ double unprofiled_goal(int row, int col) const {
+ return unprofiled_goal_(row, col);
+ }
+
// Returns the current state estimate.
const Eigen::Matrix<double, 3, 1> &X_hat() const { return loop_->X_hat(); }
double X_hat(int row, int col) const { return loop_->X_hat(row, col); }
@@ -172,6 +180,14 @@
double shoulder_angle() const { return Y_(0, 0); }
double wrist_angle() const { return Y_(1, 0) + Y_(0, 0); }
+ // Returns the unprofiled goal.
+ const Eigen::Matrix<double, 6, 1> &unprofiled_goal() const {
+ return unprofiled_goal_;
+ }
+ double unprofiled_goal(int row, int col) const {
+ return unprofiled_goal_(row, col);
+ }
+
// Returns the filtered goal.
const Eigen::Matrix<double, 6, 1> &goal() const { return loop_->R(); }
double goal(int row, int col) const { return loop_->R(row, col); }