Merge changes Ie6b43814,I6bbcafa9
* changes:
Redo timer math for spurrious events
Add scripts to modify a root filesystem for the vision pi
diff --git a/WORKSPACE b/WORKSPACE
index fb6e2cb..f14f10b 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -641,6 +641,13 @@
urls = ["http://www.frc971.org/Build-Dependencies/small_sample_logfile.fbs"],
)
+http_file(
+ name = "drivetrain_replay",
+ downloaded_file_path = "spinning_wheels_while_still.bfbs",
+ sha256 = "3fa56d9af0852798bdd00ea5cc02f8261ad2a389a12a054ba619f9f7c43ab6fd",
+ urls = ["http://www.frc971.org/Build-Dependencies/spinning_wheels_while_still.bfbs"],
+)
+
# OpenCV armhf (for raspberry pi)
http_archive(
name = "opencv_armhf",
diff --git a/aos/BUILD b/aos/BUILD
index 2bd49c1..12d25b2 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -1,5 +1,5 @@
load("//tools:environments.bzl", "mcu_cpus")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_py_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_py_library", "flatbuffer_ts_library")
filegroup(
name = "prime_binaries",
@@ -285,6 +285,12 @@
visibility = ["//visibility:public"],
)
+flatbuffer_ts_library(
+ name = "configuration_ts_fbs",
+ srcs = ["configuration.fbs"],
+ visibility = ["//visibility:public"],
+)
+
flatbuffer_py_library(
name = "configuration_fbs_python",
srcs = ["configuration.fbs"],
diff --git a/aos/network/BUILD b/aos/network/BUILD
index 96d561f..89853f0 100644
--- a/aos/network/BUILD
+++ b/aos/network/BUILD
@@ -13,6 +13,14 @@
],
)
+flatbuffer_ts_library(
+ name = "connect_ts_fbs",
+ srcs = ["connect.fbs"],
+ includes = [
+ "//aos:configuration_fbs_includes",
+ ],
+)
+
flatbuffer_cc_library(
name = "timestamp_fbs",
srcs = ["timestamp.fbs"],
diff --git a/aos/network/web_proxy.cc b/aos/network/web_proxy.cc
index 77371eb..f4a8ce8 100644
--- a/aos/network/web_proxy.cc
+++ b/aos/network/web_proxy.cc
@@ -212,6 +212,10 @@
const message_bridge::Connect *message =
flatbuffers::GetRoot<message_bridge::Connect>(buffer.data.data());
for (auto &subscriber : subscribers_) {
+ // Make sure the subscriber is for a channel on this node.
+ if (subscriber.get() == nullptr) {
+ continue;
+ }
bool found_match = false;
for (auto channel : *message->channels_to_transfer()) {
if (subscriber->Compare(channel)) {
diff --git a/aos/network/web_proxy_main.cc b/aos/network/web_proxy_main.cc
index 21b6f9c..479320a 100644
--- a/aos/network/web_proxy_main.cc
+++ b/aos/network/web_proxy_main.cc
@@ -15,13 +15,18 @@
std::vector<std::unique_ptr<aos::web_proxy::Subscriber>> *subscribers,
const aos::FlatbufferDetachedBuffer<aos::Configuration> &config) {
aos::ShmEventLoop event_loop(&config.message());
+ const aos::Node *self = aos::configuration::GetMyNode(&config.message());
// TODO(alex): skip fetchers on the wrong node.
for (uint i = 0; i < config.message().channels()->size(); ++i) {
auto channel = config.message().channels()->Get(i);
- auto fetcher = event_loop.MakeRawFetcher(channel);
- subscribers->emplace_back(
- std::make_unique<aos::web_proxy::Subscriber>(std::move(fetcher), i));
+ if (!aos::configuration::ChannelIsReadableOnNode(channel, self)) {
+ subscribers->emplace_back(nullptr);
+ } else {
+ auto fetcher = event_loop.MakeRawFetcher(channel);
+ subscribers->emplace_back(
+ std::make_unique<aos::web_proxy::Subscriber>(std::move(fetcher), i));
+ }
}
flatbuffers::FlatBufferBuilder fbb(1024);
diff --git a/aos/network/www/BUILD b/aos/network/www/BUILD
index 76e8ef4..bab5198 100644
--- a/aos/network/www/BUILD
+++ b/aos/network/www/BUILD
@@ -18,6 +18,8 @@
],
deps = [
"//aos/network:web_proxy_ts_fbs",
+ "//aos/network:connect_ts_fbs",
+ "//aos:configuration_ts_fbs",
],
visibility=["//visibility:public"],
)
diff --git a/aos/network/www/config_handler.ts b/aos/network/www/config_handler.ts
index 0af78b6..72b496e 100644
--- a/aos/network/www/config_handler.ts
+++ b/aos/network/www/config_handler.ts
@@ -1,10 +1,11 @@
-import {aos} from 'aos/network/web_proxy_generated';
+import {Configuration, Channel} from 'aos/configuration_generated';
+import {Connect} from 'aos/network/connect_generated';
export class ConfigHandler {
private readonly root_div = document.getElementById('config');
constructor(
- private readonly config: aos.Configuration,
+ private readonly config: Configuration,
private readonly dataChannel: RTCDataChannel) {}
printConfig() {
@@ -46,19 +47,18 @@
const channel = this.config.channels(index);
const namefb = builder.createString(channel.name());
const typefb = builder.createString(channel.type());
- aos.Channel.startChannel(builder);
- aos.Channel.addName(builder, namefb);
- aos.Channel.addType(builder, typefb);
- const channelfb = aos.Channel.endChannel(builder);
+ Channel.startChannel(builder);
+ Channel.addName(builder, namefb);
+ Channel.addType(builder, typefb);
+ const channelfb = Channel.endChannel(builder);
channels.push(channelfb);
}
const channelsfb =
- aos.message_bridge.Connect.createChannelsToTransferVector(
- builder, channels);
- aos.message_bridge.Connect.startConnect(builder);
- aos.message_bridge.Connect.addChannelsToTransfer(builder, channelsfb);
- const connect = aos.message_bridge.Connect.endConnect(builder);
+ Connect.createChannelsToTransferVector(builder, channels);
+ Connect.startConnect(builder);
+ Connect.addChannelsToTransfer(builder, channelsfb);
+ const connect = Connect.endConnect(builder);
builder.finish(connect);
const array = builder.asUint8Array();
console.log('connect', array);
diff --git a/aos/network/www/ping_handler.ts b/aos/network/www/ping_handler.ts
index 9b37d70..df7635d 100644
--- a/aos/network/www/ping_handler.ts
+++ b/aos/network/www/ping_handler.ts
@@ -1,8 +1,8 @@
-import {aos} from 'aos/events/ping_generated';
+import {Ping} from 'aos/events/ping_generated';
export function HandlePing(data: Uint8Array) {
const fbBuffer = new flatbuffers.ByteBuffer(data);
- const ping = aos.examples.Ping.getRootAsPing(fbBuffer);
+ const ping = Ping.getRootAsPing(fbBuffer);
document.getElementById('val').innerHTML = ping.value();
document.getElementById('time').innerHTML = ping.sendTime().low;
diff --git a/aos/network/www/proxy.ts b/aos/network/www/proxy.ts
index 27ffbfe..80c71b2 100644
--- a/aos/network/www/proxy.ts
+++ b/aos/network/www/proxy.ts
@@ -1,5 +1,5 @@
-import {aos} from 'aos/network/web_proxy_generated';
import {ConfigHandler} from './config_handler';
+import {Configuration} from 'aos/configuration_generated';
// There is one handler for each DataChannel, it maintains the state of
// multi-part messages and delegates to a callback when the message is fully
@@ -16,7 +16,7 @@
handleMessage(e: MessageEvent): void {
const fbBuffer = new flatbuffers.ByteBuffer(new Uint8Array(e.data));
const messageHeader =
- aos.web_proxy.MessageHeader.getRootAsMessageHeader(fbBuffer);
+ WebProxy.MessageHeader.getRootAsMessageHeader(fbBuffer);
// Short circuit if only one packet
if (messageHeader.packetCount === 1) {
this.handlerFunc(messageHeader.dataArray());
@@ -44,8 +44,8 @@
private rtcPeerConnection: RTCPeerConnection|null = null;
private dataChannel: DataChannel|null = null;
private webSocketUrl: string;
- private configHandler: ConfigHandler|null =
- null private config: aos.Configuration|null = null;
+ private configHandler: ConfigHandler|null = null;
+ private config: Configuration|null = null;
private readonly handlerFuncs = new Map<string, (data: Uint8Array) => void>();
private readonly handlers = new Set<Handler>();
@@ -98,10 +98,10 @@
const candidateString = builder.createString(candidate.candidate);
const sdpMidString = builder.createString(candidate.sdpMid);
- const iceFb = aos.web_proxy.WebSocketIce.createWebSocketIce(
+ const iceFb = WebProxy.WebSocketIce.createWebSocketIce(
builder, candidateString, sdpMidString, candidate.sdpMLineIndex);
- const messageFb = aos.web_proxy.WebSocketMessage.createWebSocketMessage(
- builder, aos.web_proxy.Payload.WebSocketIce, iceFb);
+ const messageFb = WebProxy.WebSocketMessage.createWebSocketMessage(
+ builder, WebProxy.Payload.WebSocketIce, iceFb);
builder.finish(messageFb);
const array = builder.asUint8Array();
this.webSocketConnection.send(array.buffer.slice(array.byteOffset));
@@ -113,10 +113,10 @@
const builder = new flatbuffers.Builder(512);
const offerString = builder.createString(description.sdp);
- const webSocketSdp = aos.web_proxy.WebSocketSdp.createWebSocketSdp(
- builder, aos.web_proxy.SdpType.OFFER, offerString);
- const message = aos.web_proxy.WebSocketMessage.createWebSocketMessage(
- builder, aos.web_proxy.Payload.WebSocketSdp, webSocketSdp);
+ const webSocketSdp = WebProxy.WebSocketSdp.createWebSocketSdp(
+ builder, WebProxy.SdpType.OFFER, offerString);
+ const message = WebProxy.WebSocketMessage.createWebSocketMessage(
+ builder, WebProxy.Payload.WebSocketSdp, webSocketSdp);
builder.finish(message);
const array = builder.asUint8Array();
this.webSocketConnection.send(array.buffer.slice(array.byteOffset));
@@ -145,19 +145,19 @@
const buffer = new Uint8Array(e.data)
const fbBuffer = new flatbuffers.ByteBuffer(buffer);
const message =
- aos.web_proxy.WebSocketMessage.getRootAsWebSocketMessage(fbBuffer);
+ WebProxy.WebSocketMessage.getRootAsWebSocketMessage(fbBuffer);
switch (message.payloadType()) {
- case aos.web_proxy.Payload.WebSocketSdp:
- const sdpFb = message.payload(new aos.web_proxy.WebSocketSdp());
- if (sdpFb.type() !== aos.web_proxy.SdpType.ANSWER) {
+ case WebProxy.Payload.WebSocketSdp:
+ const sdpFb = message.payload(new WebProxy.WebSocketSdp());
+ if (sdpFb.type() !== WebProxy.SdpType.ANSWER) {
console.log('got something other than an answer back');
break;
}
this.rtcPeerConnection.setRemoteDescription(new RTCSessionDescription(
{'type': 'answer', 'sdp': sdpFb.payload()}));
break;
- case aos.web_proxy.Payload.WebSocketIce:
- const iceFb = message.payload(new aos.web_proxy.WebSocketIce());
+ case WebProxy.Payload.WebSocketIce:
+ const iceFb = message.payload(new WebProxy.WebSocketIce());
const candidate = {} as RTCIceCandidateInit;
candidate.candidate = iceFb.candidate();
candidate.sdpMid = iceFb.sdpMid();
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 2c94473..9fef8f9 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -152,6 +152,7 @@
hdrs = ["hybrid_ekf.h"],
deps = [
":drivetrain_config",
+ "//aos:math",
"//aos/containers:priority_queue",
"//aos/util:math",
"//frc971/control_loops:c2d",
@@ -184,7 +185,9 @@
hdrs = ["localizer.h"],
deps = [
":drivetrain_config",
+ ":drivetrain_status_fbs",
":hybrid_ekf",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:pose",
],
)
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 7dfd0e6..a19f57b 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -243,7 +243,9 @@
}
localizer_->Update({last_last_left_voltage_, last_last_right_voltage_},
monotonic_now, position->left_encoder(),
- position->right_encoder(), last_gyro_rate_, last_accel_);
+ position->right_encoder(),
+ down_estimator_.avg_recent_yaw_rates(),
+ down_estimator_.avg_recent_accel());
}
dt_openloop_.SetPosition(position, left_gear_, right_gear_);
@@ -321,6 +323,9 @@
const flatbuffers::Offset<DownEstimatorState> down_estimator_state_offset =
down_estimator_.PopulateStatus(status->fbb(), monotonic_now);
+ const flatbuffers::Offset<LocalizerState> localizer_offset =
+ localizer_->PopulateStatus(status->fbb());
+
const flatbuffers::Offset<ImuZeroerState> zeroer_offset =
imu_zeroer_.PopulateStatus(status->fbb());
@@ -363,6 +368,7 @@
builder.add_line_follow_logging(line_follow_logging_offset);
builder.add_trajectory_logging(trajectory_logging_offset);
builder.add_down_estimator(down_estimator_state_offset);
+ builder.add_localizer(localizer_offset);
builder.add_zeroing(zeroer_offset);
status->Send(builder.Finish());
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index f7924c5..b766db8 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -38,6 +38,8 @@
LocalizerInterface *localizer,
const ::std::string &name = "/drivetrain");
+ virtual ~DrivetrainLoop() {}
+
int ControllerIndexFromGears();
protected:
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 1de5d40..3c206b3 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -82,9 +82,9 @@
void VerifyNearGoal() {
drivetrain_goal_fetcher_.Fetch();
EXPECT_NEAR(drivetrain_goal_fetcher_->left_goal(),
- drivetrain_plant_.GetLeftPosition(), 1e-3);
+ drivetrain_plant_.GetLeftPosition(), 1e-2);
EXPECT_NEAR(drivetrain_goal_fetcher_->right_goal(),
- drivetrain_plant_.GetRightPosition(), 1e-3);
+ drivetrain_plant_.GetRightPosition(), 1e-2);
}
void VerifyNearPosition(double x, double y) {
@@ -100,8 +100,8 @@
const double expected_y =
CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y();
const ::Eigen::Vector2d actual = drivetrain_plant_.GetPosition();
- EXPECT_NEAR(actual(0), expected_x, 3e-2);
- EXPECT_NEAR(actual(1), expected_y, 3e-2);
+ EXPECT_NEAR(actual(0), expected_x, 4e-2);
+ EXPECT_NEAR(actual(1), expected_y, 4e-2);
}
void WaitForTrajectoryPlan() {
@@ -131,7 +131,7 @@
drivetrain_status_fetcher_->theta();
EXPECT_LT(
std::abs(aos::math::DiffAngle(down_estimator_yaw, localizer_yaw)),
- 1e-5);
+ 1e-2);
const double true_yaw = (drivetrain_plant_.GetRightPosition() -
drivetrain_plant_.GetLeftPosition()) /
(dt_config_.robot_radius * 2.0);
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 3f3a7f7..d2c0be3 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -4,6 +4,7 @@
#include <chrono>
#include "Eigen/Dense"
+#include "aos/commonmath.h"
#include "aos/containers/priority_queue.h"
#include "aos/util/math.h"
#include "frc971/control_loops/c2d.h"
@@ -37,24 +38,104 @@
// measurement updates with an encoder/gyro as well as a more generic
// update function that can be used for arbitrary nonlinear updates (presumably
// a camera update).
+//
+// Discussion of the model:
+// In the current model, we try to rely primarily on IMU measurements for
+// estimating robot state--we also need additional information (some combination
+// of output voltages, encoders, and camera data) to help eliminate the biases
+// that can accumulate due to integration of IMU data.
+// We use IMU measurements as inputs rather than measurement outputs because
+// that seemed to be easier to implement. I tried initially running with
+// the IMU as a measurement, but it seemed to blow up the complexity of the
+// model.
+//
+// On each prediction update, we take in inputs of the left/right voltages and
+// the current measured longitudinal/lateral accelerations. In the current
+// setup, the accelerometer readings will be used for estimating how the
+// evolution of the longitudinal/lateral velocities. The voltages (and voltage
+// errors) will solely be used for estimating the current rotational velocity of
+// the robot (I do this because currently I suspect that the accelerometer is a
+// much better indicator of current robot state than the voltages). We also
+// deliberately decay all of the velocity estimates towards zero to help address
+// potential accelerometer biases. We use two separate decay models:
+// -The longitudinal velocity is modelled as decaying at a constant rate (see
+// the documentation on the VelocityAccel() method)--this needs a more
+// complex model because the robot will, under normal circumstances, be
+// travelling at non-zero velocities.
+// -The lateral velocity is modelled as exponentially decaying towards zero.
+// This is simpler to model and should be reasonably valid, since we will
+// not *normally* be travelling sideways consistently (this assumption may
+// need to be revisited).
+// -The "longitudinal velocity offset" (described below) also uses an
+// exponential decay, albeit with a different time constant. A future
+// improvement may remove the decay modelling on the longitudinal velocity
+// itself and instead use that decay model on the longitudinal velocity offset.
+// This would place a bit more trust in the encoder measurements but also
+// more correctly model situations where the robot is legitimately moving at
+// a certain velocity.
+//
+// For modelling how the drivetrain encoders evolve, and to help prevent the
+// aforementioned decay functions from affecting legitimate high-velocity
+// maneuvers too much, we have a "longitudinal velocity offset" term. This term
+// models the difference between the actual longitudinal velocity of the robot
+// (estimated by the average of the left/right velocities) and the velocity
+// experienced by the wheels (which can be observed from the encoders more
+// directly). Because we model this velocity offset as decaying towards zero,
+// what this will do is allow the encoders to be a constant velocity off from
+// the accelerometer updates for short periods of time but then gradually
+// pull the "actual" longitudinal velocity offset towards that of the encoders,
+// helping to reduce constant biases.
template <typename Scalar = double>
class HybridEkf {
public:
// An enum specifying what each index in the state vector is for.
enum StateIdx {
+ // Current X/Y position, in meters, of the robot.
kX = 0,
kY = 1,
+ // Current heading of the robot.
kTheta = 2,
+ // Current estimated encoder reading of the left wheels, in meters.
+ // Rezeroed once on startup.
kLeftEncoder = 3,
+ // Current estimated actual velocity of the left side of the robot, in m/s.
kLeftVelocity = 4,
+ // Same variables, for the right side of the robot.
kRightEncoder = 5,
kRightVelocity = 6,
+ // Estimated offset to input voltage. Used as a generic error term, Volts.
kLeftVoltageError = 7,
kRightVoltageError = 8,
+ // These error terms are used to estimate the difference between the actual
+ // movement of the drivetrain and that implied by the wheel odometry.
+ // Angular error effectively estimates a constant angular rate offset of the
+ // encoders relative to the actual rotation of the robot.
+ // Semi-arbitrary units (we don't bother accounting for robot radius in
+ // this).
kAngularError = 9,
+ // Estimate of slip between the drivetrain wheels and the actual
+ // forwards/backwards velocity of the robot, in m/s.
+ // I.e., (left velocity + right velocity) / 2.0 = (left wheel velocity +
+ // right wheel velocity) / 2.0 + longitudinal velocity offset
+ kLongitudinalVelocityOffset = 10,
+ // Current estimate of the lateral velocity of the robot, in m/s.
+ // Positive implies the robot is moving to its left.
+ kLateralVelocity = 11,
};
- static constexpr int kNStates = 10;
- static constexpr int kNInputs = 2;
+ static constexpr int kNStates = 12;
+ enum InputIdx {
+ // Left/right drivetrain voltages.
+ kLeftVoltage = 0,
+ kRightVoltage = 1,
+ // Current accelerometer readings, in m/s/s, along the longitudinal and
+ // lateral axes of the robot. Should be projected onto the X/Y plane, to
+ // compensate for tilt of the robot before being passed to this filter. The
+ // HybridEkf has no knowledge of the current pitch/roll of the robot, and so
+ // can't do anything to compensate for it.
+ kLongitudinalAccel = 2,
+ kLateralAccel = 3,
+ };
+ static constexpr int kNInputs = 4;
// Number of previous samples to save.
static constexpr int kSaveSamples = 50;
// Assume that all correction steps will have kNOutputs
@@ -63,6 +144,10 @@
// figuring out how to deal with storing variable size
// observation matrices, though.
static constexpr int kNOutputs = 3;
+ // Time constant to use for estimating how the longitudinal/lateral velocity
+ // offsets decay, in seconds.
+ static constexpr double kVelocityOffsetTimeConstant = 10.0;
+ static constexpr double kLateralVelocityTimeConstant = 1.0;
// Inputs are [left_volts, right_volts]
typedef Eigen::Matrix<Scalar, kNInputs, 1> Input;
// Outputs are either:
@@ -71,19 +156,7 @@
// variable-size measurement updates.
typedef Eigen::Matrix<Scalar, kNOutputs, 1> Output;
typedef Eigen::Matrix<Scalar, kNStates, kNStates> StateSquare;
- // State is [x_position, y_position, theta, Kalman States], where
- // Kalman States are the states from the standard drivetrain Kalman Filter,
- // which is: [left encoder, left ground vel, right encoder, right ground vel,
- // left voltage error, right voltage error, angular_error], where:
- // left/right encoder should correspond directly to encoder readings
- // left/right velocities are the velocity of the left/right sides over the
- // ground (i.e., corrected for angular_error).
- // voltage errors are the difference between commanded and effective voltage,
- // used to estimate consistent modelling errors (e.g., friction).
- // angular error is the difference between the angular velocity as estimated
- // by the encoders vs. estimated by the gyro, such as might be caused by
- // wheels on one side of the drivetrain being too small or one side's
- // wheels slipping more than the other.
+ // State contains the states defined by the StateIdx enum. See comments there.
typedef Eigen::Matrix<Scalar, kNStates, 1> State;
// Constructs a HybridEkf for a particular drivetrain.
@@ -136,28 +209,39 @@
// matrix for linear cases?
void Correct(
const Output &z, const Input *U,
- ::std::function<
+ std::function<
void(const State &, const StateSquare &,
- ::std::function<Output(const State &, const Input &)> *,
- ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
- const State &)> *)>
- make_h,
- ::std::function<Output(const State &, const Input &)> h,
- ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
- dhdx,
- const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
+ std::function<Output(const State &, const Input &)> *,
+ std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
+ const State &)> *)> make_h,
+ std::function<Output(const State &, const Input &)> h,
+ std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
+ dhdx, const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
aos::monotonic_clock::time_point t);
// A utility function for specifically updating with encoder and gyro
// measurements.
void UpdateEncodersAndGyro(const Scalar left_encoder,
const Scalar right_encoder, const Scalar gyro_rate,
- const Input &U,
- ::aos::monotonic_clock::time_point t) {
+ const Eigen::Matrix<Scalar, 2, 1> &voltage,
+ const Eigen::Matrix<Scalar, 3, 1> &accel,
+ aos::monotonic_clock::time_point t) {
+ Input U;
+ U.template block<2, 1>(0, 0) = voltage;
+ U.template block<2, 1>(kLongitudinalAccel, 0) =
+ accel.template block<2, 1>(0, 0);
+ RawUpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U, t);
+ }
+ // Version of UpdateEncodersAndGyro that takes a input matrix rather than
+ // taking in a voltage/acceleration separately.
+ void RawUpdateEncodersAndGyro(const Scalar left_encoder,
+ const Scalar right_encoder,
+ const Scalar gyro_rate, const Input &U,
+ aos::monotonic_clock::time_point t) {
// Because the check below for have_zeroed_encoders_ will add an
// Observation, do a check here to ensure that initialization has been
// performed and so there is at least one observation.
- AOS_CHECK(!observations_.empty());
+ CHECK(!observations_.empty());
if (!have_zeroed_encoders_) {
// This logic handles ensuring that on the first encoder reading, we
// update the internal state for the encoders to match the reading.
@@ -165,35 +249,49 @@
// wpilib_interface, then we can get some obnoxious initial corrections
// that mess up the localization.
State newstate = X_hat_;
- newstate(kLeftEncoder, 0) = left_encoder;
- newstate(kRightEncoder, 0) = right_encoder;
- newstate(kLeftVoltageError, 0) = 0.0;
- newstate(kRightVoltageError, 0) = 0.0;
- newstate(kAngularError, 0) = 0.0;
+ newstate(kLeftEncoder) = left_encoder;
+ newstate(kRightEncoder) = right_encoder;
+ newstate(kLeftVoltageError) = 0.0;
+ newstate(kRightVoltageError) = 0.0;
+ newstate(kAngularError) = 0.0;
+ newstate(kLongitudinalVelocityOffset) = 0.0;
+ newstate(kLateralVelocity) = 0.0;
ResetInitialState(t, newstate, P_);
// We need to set have_zeroed_encoders_ after ResetInitialPosition because
// the reset clears have_zeroed_encoders_...
have_zeroed_encoders_ = true;
}
+
Output z(left_encoder, right_encoder, gyro_rate);
+
Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R;
R.setZero();
R.diagonal() << encoder_noise_, encoder_noise_, gyro_noise_;
- Correct(z, &U, {},
- [this](const State &X, const Input &) {
- return H_encoders_and_gyro_ * X;
- },
- [this](const State &) { return H_encoders_and_gyro_; }, R, t);
+ Correct(
+ z, &U, {},
+ [this](const State &X, const Input &) {
+ return H_encoders_and_gyro_ * X;
+ },
+ [this](const State &) { return H_encoders_and_gyro_; }, R, t);
}
// Sundry accessor:
State X_hat() const { return X_hat_; }
- Scalar X_hat(long i) const { return X_hat_(i, 0); }
+ Scalar X_hat(long i) const { return X_hat_(i); }
StateSquare P() const { return P_; }
- ::aos::monotonic_clock::time_point latest_t() const {
+ aos::monotonic_clock::time_point latest_t() const {
return observations_.top().t;
}
+ static Scalar CalcLongitudinalVelocity(const State &X) {
+ return (X(kLeftVelocity) + X(kRightVelocity)) / 2.0;
+ }
+
+ Scalar CalcYawRate(const State &X) const {
+ return (X(kRightVelocity) - X(kLeftVelocity)) / 2.0 /
+ dt_config_.robot_radius;
+ }
+
private:
struct Observation {
// Time when the observation was taken.
@@ -213,16 +311,14 @@
// estimate. This is used by the camera to make it so that we only have to
// match targets once.
// Only called if h and dhdx are empty.
- ::std::function<void(
- const State &, const StateSquare &,
- ::std::function<Output(const State &, const Input &)> *,
- ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
- const State &)> *)>
- make_h;
+ std::function<void(const State &, const StateSquare &,
+ std::function<Output(const State &, const Input &)> *,
+ std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
+ const State &)> *)> make_h;
// A function to calculate the expected output at a given state/input.
// TODO(james): For encoders/gyro, it is linear and the function call may
// be expensive. Potential source of optimization.
- ::std::function<Output(const State &, const Input &)> h;
+ std::function<Output(const State &, const Input &)> h;
// The Jacobian of h with respect to x.
// We assume that U has no impact on the Jacobian.
// TODO(james): Currently, none of the users of this actually make use of
@@ -230,7 +326,7 @@
// recalculate it to be strictly correct, but I was both too lazy to do
// so and it seemed unnecessary). This is a potential source for future
// optimizations if function calls are being expensive.
- ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
+ std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
dhdx;
// The measurement noise matrix.
Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R;
@@ -244,33 +340,116 @@
void InitializeMatrices();
- StateSquare AForState(const State &X) const {
+ // These constants and functions define how the longitudinal velocity
+ // (the average of the left and right velocities) decays. We model it as
+ // decaying at a constant rate, except very near zero where the decay rate is
+ // exponential (this is more numerically stable than just using a constant
+ // rate the whole time). We use this model rather than a simpler exponential
+ // decay because an exponential decay will result in the robot's velocity
+ // estimate consistently being far too low when at high velocities, and since
+ // the acceleromater-based estimate of the velocity will only drift at a
+ // relatively slow rate and doesn't get worse at higher velocities, we can
+ // safely decay pretty slowly.
+ static constexpr double kMaxVelocityAccel = 0.005;
+ static constexpr double kMaxVelocityGain = 1.0;
+ static Scalar VelocityAccel(Scalar velocity) {
+ return -std::clamp(kMaxVelocityGain * velocity, -kMaxVelocityAccel,
+ kMaxVelocityAccel);
+ }
+
+ static Scalar VelocityAccelDiff(Scalar velocity) {
+ return (std::abs(kMaxVelocityGain * velocity) > kMaxVelocityAccel)
+ ? 0.0
+ : -kMaxVelocityGain;
+ }
+
+ // Returns the "A" matrix for a given state. See DiffEq for discussion of
+ // ignore_accel.
+ StateSquare AForState(const State &X, bool ignore_accel = false) const {
+ // Calculate the A matrix for a given state. Note that A = partial Xdot /
+ // partial X. This is distinct from saying that Xdot = A * X. This is
+ // particularly relevant for the (kX, kTheta) members which otherwise seem
+ // odd.
StateSquare A_continuous = A_continuous_;
- const Scalar theta = X(kTheta, 0);
- const Scalar linear_vel =
- (X(kLeftVelocity, 0) + X(kRightVelocity, 0)) / 2.0;
- const Scalar stheta = ::std::sin(theta);
- const Scalar ctheta = ::std::cos(theta);
+ const Scalar theta = X(kTheta);
+ const Scalar stheta = std::sin(theta);
+ const Scalar ctheta = std::cos(theta);
+ const Scalar lng_vel = CalcLongitudinalVelocity(X);
+ const Scalar lat_vel = X(kLateralVelocity);
+ const Scalar diameter = 2.0 * dt_config_.robot_radius;
+ const Scalar yaw_rate = CalcYawRate(X);
// X and Y derivatives
- A_continuous(kX, kTheta) = -stheta * linear_vel;
+ A_continuous(kX, kTheta) =
+ -stheta * lng_vel - ctheta * lat_vel;
A_continuous(kX, kLeftVelocity) = ctheta / 2.0;
A_continuous(kX, kRightVelocity) = ctheta / 2.0;
- A_continuous(kY, kTheta) = ctheta * linear_vel;
+ A_continuous(kX, kLateralVelocity) = -stheta;
+ A_continuous(kY, kTheta) = ctheta * lng_vel - stheta * lat_vel;
A_continuous(kY, kLeftVelocity) = stheta / 2.0;
A_continuous(kY, kRightVelocity) = stheta / 2.0;
+ A_continuous(kY, kLateralVelocity) = ctheta;
+
+ if (!ignore_accel) {
+ const Eigen::Matrix<Scalar, 1, kNStates> lng_vel_row =
+ (A_continuous.row(kLeftVelocity) + A_continuous.row(kRightVelocity)) /
+ 2.0;
+ A_continuous.row(kLeftVelocity) -= lng_vel_row;
+ A_continuous.row(kRightVelocity) -= lng_vel_row;
+ // Terms to account for centripetal accelerations.
+ // lateral centripetal accel = -yaw_rate * lng_vel
+ A_continuous(kLateralVelocity, kLeftVelocity) +=
+ X(kLeftVelocity) / diameter;
+ A_continuous(kLateralVelocity, kRightVelocity) +=
+ -X(kRightVelocity) / diameter;
+ A_continuous(kRightVelocity, kLateralVelocity) += yaw_rate;
+ A_continuous(kLeftVelocity, kLateralVelocity) += yaw_rate;
+ const Scalar dlng_accel_dwheel_vel = X(kLateralVelocity) / diameter;
+ A_continuous(kRightVelocity, kRightVelocity) += dlng_accel_dwheel_vel;
+ A_continuous(kLeftVelocity, kRightVelocity) += dlng_accel_dwheel_vel;
+ A_continuous(kRightVelocity, kLeftVelocity) += -dlng_accel_dwheel_vel;
+ A_continuous(kLeftVelocity, kLeftVelocity) += -dlng_accel_dwheel_vel;
+
+ A_continuous(kRightVelocity, kRightVelocity) +=
+ VelocityAccelDiff(lng_vel) / 2.0;
+ A_continuous(kRightVelocity, kLeftVelocity) +=
+ VelocityAccelDiff(lng_vel) / 2.0;
+ A_continuous(kLeftVelocity, kRightVelocity) +=
+ VelocityAccelDiff(lng_vel) / 2.0;
+ A_continuous(kLeftVelocity, kLeftVelocity) +=
+ VelocityAccelDiff(lng_vel) / 2.0;
+ }
return A_continuous;
}
- State DiffEq(const State &X, const Input &U) const {
+ // Returns dX / dt given X and U. If ignore_accel is set, then we ignore the
+ // accelerometer-based components of U (this is solely used in testing).
+ State DiffEq(const State &X, const Input &U,
+ bool ignore_accel = false) const {
State Xdot = A_continuous_ * X + B_continuous_ * U;
// And then we need to add on the terms for the x/y change:
- const Scalar theta = X(kTheta, 0);
- const Scalar linear_vel =
- (X(kLeftVelocity, 0) + X(kRightVelocity, 0)) / 2.0;
- const Scalar stheta = ::std::sin(theta);
- const Scalar ctheta = ::std::cos(theta);
- Xdot(kX, 0) = ctheta * linear_vel;
- Xdot(kY, 0) = stheta * linear_vel;
+ const Scalar theta = X(kTheta);
+ const Scalar lng_vel = CalcLongitudinalVelocity(X);
+ const Scalar lat_vel = X(kLateralVelocity);
+ const Scalar stheta = std::sin(theta);
+ const Scalar ctheta = std::cos(theta);
+ Xdot(kX) = ctheta * lng_vel - stheta * lat_vel;
+ Xdot(kY) = stheta * lng_vel + ctheta * lat_vel;
+
+ const Scalar yaw_rate = CalcYawRate(X);
+ const Scalar expected_lat_accel = lng_vel * yaw_rate;
+ const Scalar expected_lng_accel =
+ CalcLongitudinalVelocity(Xdot) - yaw_rate * lat_vel;
+ const Scalar lng_accel_offset =
+ U(kLongitudinalAccel) - expected_lng_accel;
+ constexpr double kAccelWeight = 1.0;
+ if (!ignore_accel) {
+ Xdot(kLeftVelocity) += kAccelWeight * lng_accel_offset;
+ Xdot(kRightVelocity) += kAccelWeight * lng_accel_offset;
+ Xdot(kLateralVelocity) += U(kLateralAccel) - expected_lat_accel;
+
+ Xdot(kRightVelocity) += VelocityAccel(lng_vel);
+ Xdot(kLeftVelocity) += VelocityAccel(lng_vel);
+ }
return Xdot;
}
@@ -283,7 +462,7 @@
*state = RungeKuttaU(
[this](const State &X, const Input &U) { return DiffEq(X, U); }, *state,
- U, ::aos::time::DurationInSeconds(dt));
+ U, aos::time::DurationInSeconds(dt));
StateSquare Ptemp = A_d * *P * A_d.transpose() + Q_d;
*P = Ptemp;
@@ -310,7 +489,7 @@
PredictImpl(obs->U, dt, state, P);
}
if (!(obs->h && obs->dhdx)) {
- AOS_CHECK(obs->make_h);
+ CHECK(obs->make_h);
obs->make_h(*state, *P, &obs->h, &obs->dhdx);
}
CorrectImpl(obs->R, obs->z, obs->h(*state, obs->U), obs->dhdx(*state),
@@ -330,45 +509,42 @@
bool have_zeroed_encoders_ = false;
- aos::PriorityQueue<Observation, kSaveSamples, ::std::less<Observation>>
+ aos::PriorityQueue<Observation, kSaveSamples, std::less<Observation>>
observations_;
+
friend class testing::HybridEkfTest;
- friend class ::y2019::control_loops::testing::ParameterizedLocalizerTest;
+ friend class y2019::control_loops::testing::ParameterizedLocalizerTest;
}; // class HybridEkf
template <typename Scalar>
void HybridEkf<Scalar>::Correct(
const Output &z, const Input *U,
- ::std::function<
- void(const State &, const StateSquare &,
- ::std::function<Output(const State &, const Input &)> *,
- ::std::function<
- Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)> *)>
- make_h,
- ::std::function<Output(const State &, const Input &)> h,
- ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
- dhdx,
- const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
+ std::function<void(const State &, const StateSquare &,
+ std::function<Output(const State &, const Input &)> *,
+ std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
+ const State &)> *)> make_h,
+ std::function<Output(const State &, const Input &)> h,
+ std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
+ dhdx, const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
aos::monotonic_clock::time_point t) {
- AOS_CHECK(!observations_.empty());
+ CHECK(!observations_.empty());
if (!observations_.full() && t < observations_.begin()->t) {
- AOS_LOG(ERROR,
- "Dropped an observation that was received before we "
- "initialized.\n");
+ LOG(ERROR) << "Dropped an observation that was received before we "
+ "initialized.\n";
return;
}
auto cur_it =
observations_.PushFromBottom({t, t, State::Zero(), StateSquare::Zero(),
Input::Zero(), z, make_h, h, dhdx, R});
if (cur_it == observations_.end()) {
- AOS_LOG(
- DEBUG,
- "Camera dropped off of end with time of %fs; earliest observation in "
- "queue has time of %fs.\n",
- ::aos::time::DurationInSeconds(t.time_since_epoch()),
- ::aos::time::DurationInSeconds(
- observations_.begin()->t.time_since_epoch()));
+ VLOG(1) << "Camera dropped off of end with time of "
+ << aos::time::DurationInSeconds(t.time_since_epoch())
+ << "s; earliest observation in "
+ "queue has time of "
+ << aos::time::DurationInSeconds(
+ observations_.begin()->t.time_since_epoch())
+ << "s.\n";
return;
}
@@ -396,7 +572,7 @@
--prev_it;
cur_it->prev_t = prev_it->t;
// TODO(james): Figure out a saner way of handling this.
- AOS_CHECK(U != nullptr);
+ CHECK(U != nullptr);
cur_it->U = *U;
} else {
cur_it->X_hat = next_it->X_hat;
@@ -412,7 +588,7 @@
// We use X_hat_ and P_ to store the intermediate states, and then
// once we reach the end they will all be up-to-date.
ProcessObservation(&*cur_it, cur_it->t - cur_it->prev_t, &X_hat_, &P_);
- AOS_CHECK(X_hat_.allFinite());
+ CHECK(X_hat_.allFinite());
if (next_it != observations_.end()) {
next_it->X_hat = X_hat_;
next_it->P = P_;
@@ -435,8 +611,10 @@
// Encoder derivatives
A_continuous_(kLeftEncoder, kLeftVelocity) = 1.0;
A_continuous_(kLeftEncoder, kAngularError) = 1.0;
+ A_continuous_(kLeftEncoder, kLongitudinalVelocityOffset) = -1.0;
A_continuous_(kRightEncoder, kRightVelocity) = 1.0;
A_continuous_(kRightEncoder, kAngularError) = -1.0;
+ A_continuous_(kRightEncoder, kLongitudinalVelocityOffset) = -1.0;
// Pull velocity derivatives from velocity matrices.
// Note that this looks really awkward (doesn't use
@@ -449,11 +627,21 @@
A_continuous_(kRightVelocity, kLeftVelocity) = vel_coefs.A_continuous(1, 0);
A_continuous_(kRightVelocity, kRightVelocity) = vel_coefs.A_continuous(1, 1);
- // Provide for voltage error terms:
+ A_continuous_(kLongitudinalVelocityOffset, kLongitudinalVelocityOffset) =
+ -1.0 / kVelocityOffsetTimeConstant;
+ A_continuous_(kLateralVelocity, kLateralVelocity) =
+ -1.0 / kLateralVelocityTimeConstant;
+
+ // We currently ignore all voltage-related modelling terms.
+ // TODO(james): Decide what to do about these terms. They don't really matter
+ // too much when we have accelerometer readings available.
B_continuous_.setZero();
- B_continuous_.row(kLeftVelocity) = vel_coefs.B_continuous.row(0);
- B_continuous_.row(kRightVelocity) = vel_coefs.B_continuous.row(1);
- A_continuous_.template block<kNStates, kNInputs>(0, 7) = B_continuous_;
+ B_continuous_.template block<1, 2>(kLeftVelocity, kLeftVoltage) =
+ vel_coefs.B_continuous.row(0);
+ B_continuous_.template block<1, 2>(kRightVelocity, kLeftVoltage) =
+ vel_coefs.B_continuous.row(1);
+ A_continuous_.template block<kNStates, 2>(0, kLeftVoltageError) =
+ B_continuous_.template block<kNStates, 2>(0, kLeftVoltage);
Q_continuous_.setZero();
// TODO(james): Improve estimates of process noise--e.g., X/Y noise can
@@ -463,16 +651,26 @@
Q_continuous_(kX, kX) = 0.002;
Q_continuous_(kY, kY) = 0.002;
Q_continuous_(kTheta, kTheta) = 0.0001;
- Q_continuous_(kLeftEncoder, kLeftEncoder) = ::std::pow(0.15, 2.0);
- Q_continuous_(kRightEncoder, kRightEncoder) = ::std::pow(0.15, 2.0);
- Q_continuous_(kLeftVelocity, kLeftVelocity) = ::std::pow(0.5, 2.0);
- Q_continuous_(kRightVelocity, kRightVelocity) = ::std::pow(0.5, 2.0);
- Q_continuous_(kLeftVoltageError, kLeftVoltageError) = ::std::pow(10.0, 2.0);
- Q_continuous_(kRightVoltageError, kRightVoltageError) = ::std::pow(10.0, 2.0);
- Q_continuous_(kAngularError, kAngularError) = ::std::pow(2.0, 2.0);
-
- P_.setZero();
- P_.diagonal() << 0.01, 0.01, 0.01, 0.02, 0.01, 0.02, 0.01, 1, 1, 0.03;
+ Q_continuous_(kLeftEncoder, kLeftEncoder) = std::pow(0.15, 2.0);
+ Q_continuous_(kRightEncoder, kRightEncoder) = std::pow(0.15, 2.0);
+ Q_continuous_(kLeftVelocity, kLeftVelocity) = std::pow(0.5, 2.0);
+ Q_continuous_(kRightVelocity, kRightVelocity) = std::pow(0.5, 2.0);
+ Q_continuous_(kLeftVoltageError, kLeftVoltageError) = std::pow(10.0, 2.0);
+ Q_continuous_(kRightVoltageError, kRightVoltageError) = std::pow(10.0, 2.0);
+ Q_continuous_(kAngularError, kAngularError) = std::pow(2.0, 2.0);
+ // This noise value largely governs whether we will trust the encoders or
+ // accelerometer more for estimating the robot position.
+ // Note that this also affects how we interpret camera measurements,
+ // particularly when using a heading/distance/skew measurement--if the
+ // noise on these numbers is particularly high, then we can end up with weird
+ // dynamics where a camera update both shifts our X/Y position and adjusts our
+ // velocity estimates substantially, causing the camera updates to create
+ // "momentum" and if we don't trust the encoders enough, then we have no way of
+ // determining that the velocity updates are bogus. This also interacts with
+ // kVelocityOffsetTimeConstant.
+ Q_continuous_(kLongitudinalVelocityOffset, kLongitudinalVelocityOffset) =
+ std::pow(1.1, 2.0);
+ Q_continuous_(kLateralVelocity, kLateralVelocity) = std::pow(0.1, 2.0);
H_encoders_and_gyro_.setZero();
// Encoders are stored directly in the state matrix, so are a minor
@@ -483,12 +681,8 @@
H_encoders_and_gyro_(2, kLeftVelocity) = -1.0 / diameter;
H_encoders_and_gyro_(2, kRightVelocity) = 1.0 / diameter;
- const Eigen::Matrix<Scalar, 4, 4> R_kf_drivetrain =
- dt_config_.make_kf_drivetrain_loop().observer().coefficients().R;
- // TODO(james): The multipliers here are hand-waving things that I put in when
- // tuning things. I haven't yet tried messing with these values again.
- encoder_noise_ = 0.5 * R_kf_drivetrain(0, 0);
- gyro_noise_ = 0.1 * R_kf_drivetrain(2, 2);
+ encoder_noise_ = 5e-9;
+ gyro_noise_ = 1e-13;
X_hat_.setZero();
P_.setZero();
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
index fa6acc5..bdfdd4e 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
+++ b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
@@ -14,11 +14,13 @@
namespace testing {
typedef HybridEkf<>::StateIdx StateIdx;
+typedef HybridEkf<>::InputIdx InputIdx;
+typedef HybridEkf<>::State State;
+typedef HybridEkf<>::StateSquare StateSquare;
+typedef HybridEkf<>::Input Input;
class HybridEkfTest : public ::testing::Test {
public:
- typedef HybridEkf<>::State State;
- typedef HybridEkf<>::Input Input;
::aos::testing::TestSharedMemory shm_;
HybridEkfTest()
: dt_config_(GetTestDrivetrainConfig()),
@@ -27,47 +29,84 @@
velocity_plant_coefs_(dt_config_.make_hybrid_drivetrain_velocity_loop()
.plant()
.coefficients()) {
- ekf_.ResetInitialState(t0_, State::Zero(),
- HybridEkf<>::StateSquare::Identity());
+ ekf_.ResetInitialState(t0_, State::Zero(), StateSquare::Identity());
}
protected:
- const State Update(const State &X, const Input &U) {
+ const State Update(const State &X, const Input &U, bool ignore_accel) {
return RungeKuttaU(
- ::std::bind(&HybridEkfTest::DiffEq, this, ::std::placeholders::_1,
- ::std::placeholders::_2),
- X, U, ::aos::time::DurationInSeconds(dt_config_.dt));
+ std::bind(&HybridEkfTest::DiffEq, this, std::placeholders::_1,
+ std::placeholders::_2, ignore_accel),
+ X, U, aos::time::DurationInSeconds(dt_config_.dt));
}
- void CheckDiffEq(const State &X, const Input &U) {
+ void CheckDiffEq(const State &X, const Input &U, bool ignore_accel) {
// Re-implement dynamics as a sanity check:
const double diameter = 2.0 * dt_config_.robot_radius;
- const double theta = X(StateIdx::kTheta, 0);
- const double stheta = ::std::sin(theta);
- const double ctheta = ::std::cos(theta);
- const double left_vel = X(StateIdx::kLeftVelocity, 0);
- const double right_vel = X(StateIdx::kRightVelocity, 0);
- const State Xdot_ekf = DiffEq(X, U);
- EXPECT_EQ(Xdot_ekf(StateIdx::kX, 0), ctheta * (left_vel + right_vel) / 2.0);
- EXPECT_EQ(Xdot_ekf(StateIdx::kY, 0), stheta * (left_vel + right_vel) / 2.0);
- EXPECT_EQ(Xdot_ekf(StateIdx::kTheta, 0), (right_vel - left_vel) / diameter);
- EXPECT_EQ(Xdot_ekf(StateIdx::kLeftEncoder, 0),
- left_vel + X(StateIdx::kAngularError, 0));
- EXPECT_EQ(Xdot_ekf(StateIdx::kRightEncoder, 0),
- right_vel - X(StateIdx::kAngularError, 0));
+ const double theta = X(StateIdx::kTheta);
+ const double stheta = std::sin(theta);
+ const double ctheta = std::cos(theta);
+ const double left_vel = X(StateIdx::kLeftVelocity);
+ const double right_vel = X(StateIdx::kRightVelocity);
+ const double lng_vel = (left_vel + right_vel) / 2.0;
+ const double yaw_rate =
+ (right_vel - left_vel) / 2.0 / dt_config_.robot_radius;
+ const double lat_vel = X(StateIdx::kLateralVelocity);
+ const State Xdot_ekf = DiffEq(X, U, ignore_accel);
+ EXPECT_EQ(Xdot_ekf(StateIdx::kX), ctheta * lng_vel - stheta * lat_vel);
+ EXPECT_EQ(Xdot_ekf(StateIdx::kY), stheta * lng_vel + ctheta * lat_vel);
+ EXPECT_EQ(Xdot_ekf(StateIdx::kTheta), (right_vel - left_vel) / diameter);
+ EXPECT_FLOAT_EQ(Xdot_ekf(StateIdx::kLeftEncoder),
+ left_vel + X(StateIdx::kAngularError) -
+ X(StateIdx::kLongitudinalVelocityOffset));
+ EXPECT_FLOAT_EQ(Xdot_ekf(StateIdx::kRightEncoder),
+ right_vel - X(StateIdx::kAngularError) -
+ X(StateIdx::kLongitudinalVelocityOffset));
- Eigen::Matrix<double, 2, 1> vel_x(X(StateIdx::kLeftVelocity, 0),
- X(StateIdx::kRightVelocity, 0));
- Eigen::Matrix<double, 2, 1> expected_vel_X =
+ const Eigen::Matrix<double, 2, 1> vel_x(X(StateIdx::kLeftVelocity),
+ X(StateIdx::kRightVelocity));
+ // Don't expect any contribution from the voltage terms, since we currently
+ // disable them.
+ const Eigen::Matrix<double, 2, 1> expected_accel =
velocity_plant_coefs_.A_continuous * vel_x +
velocity_plant_coefs_.B_continuous *
- (U + X.middleRows<2>(StateIdx::kLeftVoltageError));
- EXPECT_EQ(Xdot_ekf(StateIdx::kLeftVelocity, 0), expected_vel_X(0, 0));
- EXPECT_EQ(Xdot_ekf(StateIdx::kRightVelocity, 0), expected_vel_X(1, 0));
+ (U.topRows(2) + X.block<2, 1>(StateIdx::kLeftVoltageError, 0));
+ const double nominal_lng_accel =
+ (expected_accel(0) + expected_accel(1)) / 2.0;
+ if (ignore_accel) {
+ EXPECT_FLOAT_EQ(Xdot_ekf(StateIdx::kLeftVelocity), expected_accel(0));
+ EXPECT_FLOAT_EQ(Xdot_ekf(StateIdx::kRightVelocity), expected_accel(1));
+ EXPECT_EQ(Xdot_ekf(StateIdx::kLateralVelocity), 0.0);
+ EXPECT_EQ(
+ Xdot_ekf(StateIdx::kLongitudinalVelocityOffset), 0.0);
+ } else {
+ const double lng_accel = U(InputIdx::kLongitudinalAccel) +
+ lat_vel * yaw_rate + ekf_.VelocityAccel(lng_vel);
+ EXPECT_FLOAT_EQ(Xdot_ekf(StateIdx::kLeftVelocity),
+ lng_accel + expected_accel(0) - nominal_lng_accel);
+ EXPECT_FLOAT_EQ(Xdot_ekf(StateIdx::kRightVelocity),
+ lng_accel + expected_accel(1) - nominal_lng_accel);
+
+ EXPECT_FLOAT_EQ(Xdot_ekf(StateIdx::kLongitudinalVelocityOffset),
+ -X(StateIdx::kLongitudinalVelocityOffset) /
+ HybridEkf<>::kVelocityOffsetTimeConstant);
+ const double centripetal_accel = lng_vel * yaw_rate;
+ const double lat_accel = U(InputIdx::kLateralAccel) - centripetal_accel;
+ EXPECT_EQ(Xdot_ekf(StateIdx::kLateralVelocity),
+ lat_accel - X(StateIdx::kLateralVelocity) /
+ HybridEkf<>::kLateralVelocityTimeConstant);
+ }
// Dynamics don't expect error terms to change:
- EXPECT_EQ(0.0, Xdot_ekf.bottomRows<3>().squaredNorm());
+ EXPECT_EQ(0.0, Xdot_ekf(StateIdx::kLeftVoltageError));
+ EXPECT_EQ(0.0, Xdot_ekf(StateIdx::kRightVoltageError));
+ EXPECT_EQ(0.0, Xdot_ekf(StateIdx::kAngularError));
}
- State DiffEq(const State &X, const Input &U) { return ekf_.DiffEq(X, U); }
+ State DiffEq(const State &X, const Input &U, bool ignore_accel) {
+ return ekf_.DiffEq(X, U, ignore_accel);
+ }
+ StateSquare AForState(const State &X, bool ignore_accel) {
+ return ekf_.AForState(X, ignore_accel);
+ }
// Returns a random value sampled from a normal distribution with a standard
// deviation of std and a mean of zero.
@@ -75,49 +114,113 @@
DrivetrainConfig<double> dt_config_;
HybridEkf<> ekf_;
- ::aos::monotonic_clock::time_point t0_;
+ aos::monotonic_clock::time_point t0_;
StateFeedbackHybridPlantCoefficients<2, 2, 2> velocity_plant_coefs_;
- ::std::mt19937 gen_{static_cast<uint32_t>(::aos::testing::RandomSeed())};
- ::std::normal_distribution<> normal_;
+ std::mt19937 gen_{static_cast<uint32_t>(::aos::testing::RandomSeed())};
+ std::normal_distribution<> normal_;
};
-// Tests that the dynamics from DiffEq behave as expected:
-TEST_F(HybridEkfTest, CheckDynamics) {
- CheckDiffEq(State::Zero(), Input::Zero());
- CheckDiffEq(State::Zero(), {-5.0, 5.0});
- CheckDiffEq(State::Zero(), {12.0, -3.0});
- CheckDiffEq(
- (State() << 100.0, 200.0, M_PI, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0, 0.3)
- .finished(),
- {5.0, 6.0});
- CheckDiffEq(
- (State() << 100.0, 200.0, 2.0, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0, 0.3)
- .finished(),
- {5.0, 6.0});
- CheckDiffEq(
- (State() << 100.0, 200.0, -2.0, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0, 0.3)
- .finished(),
- {5.0, 6.0});
- // And check that a theta outisde of [-M_PI, M_PI] works.
- CheckDiffEq(
- (State() << 100.0, 200.0, 200.0, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0, 0.3)
- .finished(),
- {5.0, 6.0});
-}
// Tests that if we provide a bunch of observations of the position
// with zero change in time, the state should approach the estimation.
+struct DiffEqInputs {
+ State X;
+ Input U;
+ bool ignore_accel;
+};
+
+DiffEqInputs MakeDiffEqInputs(State X, Input U, bool ignore_accel) {
+ return {.X = X, .U = U, .ignore_accel = ignore_accel};
+}
+
+class HybridEkfDiffEqTest : public HybridEkfTest,
+ public ::testing::WithParamInterface<DiffEqInputs> {
+};
+
+// Tests that the dynamics from DiffEq behave as expected:
+TEST_P(HybridEkfDiffEqTest, CheckDynamics) {
+ CheckDiffEq(GetParam().X, GetParam().U, GetParam().ignore_accel);
+
+ // Calculate whether A seems to be empirically correct.
+ const StateSquare A = AForState(GetParam().X, GetParam().ignore_accel);
+ for (int ii = 0; ii < HybridEkf<>::kNStates; ++ii) {
+ SCOPED_TRACE(ii);
+ State perturbation = State::Zero();
+ constexpr double kEps = 1e-5;
+ perturbation(ii) = kEps;
+ const State Xdot_plus = DiffEq(GetParam().X + perturbation, GetParam().U,
+ GetParam().ignore_accel);
+ const State Xdot_minus = DiffEq(GetParam().X - perturbation, GetParam().U,
+ GetParam().ignore_accel);
+ const State numerical_dXdot_dX = (Xdot_plus - Xdot_minus) / (2.0 * kEps);
+ const State A_based_dXdot_dX = A * perturbation / kEps;
+ EXPECT_LT((A_based_dXdot_dX - numerical_dXdot_dX).norm(), 1e-8)
+ << "A * X: " << A_based_dXdot_dX << " numerical: " << numerical_dXdot_dX
+ << " difference: " << (A_based_dXdot_dX - numerical_dXdot_dX);
+ }
+}
+
+INSTANTIATE_TEST_CASE_P(
+ CheckMathTest, HybridEkfDiffEqTest,
+ ::testing::Values(DiffEqInputs{State::Zero(), Input::Zero(), false},
+ DiffEqInputs{State::Zero(), Input::Zero(), true},
+ DiffEqInputs{State::Zero(), {-5.0, 5.0, 0.0, 0.0}, false},
+ DiffEqInputs{State::Zero(), {-5.0, 5.0, 0.0, 0.0}, true},
+ DiffEqInputs{State::Zero(), {12.0, 3.0, 0.0, 0.0}, false},
+ DiffEqInputs{State::Zero(), {12.0, 3.0, 0.0, 0.0}, true},
+ DiffEqInputs{(State() << 100.0, 200.0, M_PI, 1.234, 0.5,
+ 1.2, 0.6, 3.0, -4.0, 0.3, 0.0, 0.0)
+ .finished(),
+ {3.0, 4.0, 5.0, 6.0},
+ false},
+ DiffEqInputs{(State() << 100.0, 200.0, M_PI, 1.234, 0.5,
+ 1.2, 0.6, 3.0, -4.0, 0.3, 0.0, 0.0)
+ .finished(),
+ {3.0, 4.0, 5.0, 6.0},
+ true},
+ DiffEqInputs{(State() << 100.0, 200.0, 2.0, 1.234, 0.5,
+ 1.2, 0.6, 3.0, -4.0, 0.3, 0.0, 0.0)
+ .finished(),
+ {3.0, 4.0, 5.0, 6.0},
+ false},
+ DiffEqInputs{(State() << 100.0, 200.0, 2.0, 1.234, 0.5,
+ 1.2, 0.6, 3.0, -4.0, 0.3, 0.0, 0.0)
+ .finished(),
+ {3.0, 4.0, 5.0, 6.0},
+ true},
+ DiffEqInputs{(State() << 100.0, 200.0, 2.0, 1.234, 0.5,
+ 1.2, 0.6, 3.0, -4.0, 0.3, 0.1, 0.2)
+ .finished(),
+ {3.0, 4.0, 5.0, 6.0},
+ false},
+ DiffEqInputs{(State() << 100.0, 200.0, -2.0, 1.234, 0.5,
+ 1.2, 0.6, 3.0, -4.0, 0.3, 0.0, 0.0)
+ .finished(),
+ {-3.0, -4.0, -5.0, -6.0},
+ false},
+ DiffEqInputs{(State() << 100.0, 200.0, -2.0, 1.234, 0.5,
+ 1.2, 0.6, 3.0, -4.0, 0.3, 0.0, 0.0)
+ .finished(),
+ {-3.0, -4.0, -5.0, -6.0},
+ true},
+ // And check that a theta outside of [-M_PI, M_PI] works.
+ DiffEqInputs{(State() << 100.0, 200.0, 200.0, 1.234, 0.5,
+ 1.2, 0.6, 3.0, -4.0, 0.3, 0.0, 0.0)
+ .finished(),
+ {3.0, 4.0, 5.0, 6.0},
+ false}));
+
TEST_F(HybridEkfTest, ZeroTimeCorrect) {
HybridEkf<>::Output Z(0.5, 0.5, 1);
- Eigen::Matrix<double, 3, 10> H;
+ Eigen::Matrix<double, 3, 12> H;
H.setIdentity();
auto h = [H](const State &X, const Input &) { return H * X; };
auto dhdx = [H](const State &) { return H; };
Eigen::Matrix<double, 3, 3> R;
R.setIdentity();
R *= 1e-3;
- Input U(0, 0);
+ Input U = Input::Zero();
EXPECT_EQ(0.0, ekf_.X_hat(StateIdx::kTheta));
const double starting_p_norm = ekf_.P().norm();
for (int ii = 0; ii < 100; ++ii) {
@@ -138,14 +241,14 @@
HybridEkf<>::Output Z(0, 0, 0);
// Use true_X to track what we think the true robot state is.
State true_X = ekf_.X_hat();
- Eigen::Matrix<double, 3, 10> H;
+ Eigen::Matrix<double, 3, 12> H;
H.setZero();
auto h = [H](const State &X, const Input &) { return H * X; };
auto dhdx = [H](const State &) { return H; };
// Provide constant input voltage.
- Eigen::Matrix<double, 2, 1> U;
- U << 12.0, 10.0;
- // The exact value of the noise matrix ix unimportant so long as it is
+ Input U;
+ U << 12.0, 10.0, 1.0, -0.1;
+ // The exact value of the noise matrix is unimportant so long as it is
// non-zero.
Eigen::Matrix<double, 3, 3> R;
R.setIdentity();
@@ -153,25 +256,23 @@
const double starting_p_norm = ekf_.P().norm();
for (int ii = 0; ii < 100; ++ii) {
ekf_.Correct(Z, &U, {}, h, dhdx, R, t0_ + dt_config_.dt * (ii + 1));
- true_X = Update(true_X, U);
+ true_X = Update(true_X, U, false);
EXPECT_EQ(true_X, ekf_.X_hat());
}
// We don't care about precise results, just that they are generally sane:
// robot should've travelled forwards and slightly to the right.
- EXPECT_NEAR(0.9, ekf_.X_hat(StateIdx::kX), 0.1);
- EXPECT_NEAR(-0.15, ekf_.X_hat(StateIdx::kY), 0.05);
- EXPECT_NEAR(-0.3, ekf_.X_hat(StateIdx::kTheta), 0.05);
- EXPECT_NEAR(1.0, ekf_.X_hat(StateIdx::kLeftEncoder), 0.1);
- EXPECT_NEAR(ekf_.X_hat(StateIdx::kLeftEncoder) * 0.8,
- ekf_.X_hat(StateIdx::kRightEncoder),
- ekf_.X_hat(StateIdx::kLeftEncoder) * 0.1);
- EXPECT_NEAR(2.4, ekf_.X_hat(StateIdx::kLeftVelocity), 0.1);
- EXPECT_NEAR(ekf_.X_hat(StateIdx::kLeftVelocity) * 0.8,
- ekf_.X_hat(StateIdx::kRightVelocity),
- ekf_.X_hat(StateIdx::kLeftVelocity) * 0.1);
+ EXPECT_NEAR(0.1, ekf_.X_hat(StateIdx::kX), 0.05);
+ EXPECT_NEAR(-0.02, ekf_.X_hat(StateIdx::kY), 0.01);
+ EXPECT_GT(0.01, ekf_.X_hat(StateIdx::kTheta));
+ EXPECT_NEAR(0.3, ekf_.X_hat(StateIdx::kLeftEncoder), 0.1);
+ EXPECT_NEAR(0.7, ekf_.X_hat(StateIdx::kLeftVelocity), 0.1);
+ EXPECT_NEAR(0.3, ekf_.X_hat(StateIdx::kRightVelocity), 0.1);
EXPECT_EQ(0.0, ekf_.X_hat(StateIdx::kLeftVoltageError));
EXPECT_EQ(0.0, ekf_.X_hat(StateIdx::kRightVoltageError));
EXPECT_EQ(0.0, ekf_.X_hat(StateIdx::kAngularError));
+ // Without encoder updates, the longitudinal velocity offset should be zero.
+ EXPECT_EQ(0.0, ekf_.X_hat(StateIdx::kLongitudinalVelocityOffset));
+ EXPECT_NEAR(0.04, ekf_.X_hat(StateIdx::kLateralVelocity), 0.01);
const double ending_p_norm = ekf_.P().norm();
// Due to lack of corrections, noise should've increased.
EXPECT_GT(ending_p_norm, starting_p_norm * 1.10);
@@ -191,12 +292,12 @@
TEST_P(HybridEkfOldCorrectionsTest, CreateOldCorrection) {
HybridEkf<>::Output Z;
Z.setZero();
- Eigen::Matrix<double, 3, 10> H;
+ Eigen::Matrix<double, 3, 12> H;
H.setZero();
auto h_zero = [H](const State &X, const Input &) { return H * X; };
auto dhdx_zero = [H](const State &) { return H; };
- Eigen::Matrix<double, 2, 1> U;
- U << 12.0, 12.0;
+ Input U;
+ U << 12.0, 12.0, 0.0, 0.0;
Eigen::Matrix<double, 3, 3> R;
R = R.Identity();
EXPECT_EQ(0.0, ekf_.X_hat().norm());
@@ -247,11 +348,12 @@
TEST_F(HybridEkfTest, DiscardTooOldCorrection) {
HybridEkf<>::Output Z;
Z.setZero();
- Eigen::Matrix<double, 3, 10> H;
+ Eigen::Matrix<double, 3, 12> H;
H.setZero();
auto h_zero = [H](const State &X, const Input &) { return H * X; };
auto dhdx_zero = [H](const State &) { return H; };
- Eigen::Matrix<double, 2, 1> U(12.0, 12.0);
+ Input U;
+ U << 12.0, 12.0, 0.0, 0.0;
Eigen::Matrix<double, 3, 3> R;
R.setIdentity();
@@ -286,15 +388,16 @@
// provided:
TEST_F(HybridEkfTest, PerfectEncoderUpdate) {
State true_X = ekf_.X_hat();
- Input U(-1.0, 5.0);
+ Input U;
+ U << -1.0, 5.0, 0.0, 0.0;
for (int ii = 0; ii < 100; ++ii) {
- true_X = Update(true_X, U);
- ekf_.UpdateEncodersAndGyro(true_X(StateIdx::kLeftEncoder, 0),
- true_X(StateIdx::kRightEncoder, 0),
- (true_X(StateIdx::kRightVelocity, 0) -
- true_X(StateIdx::kLeftVelocity, 0)) /
- dt_config_.robot_radius / 2.0,
- U, t0_ + (ii + 1) * dt_config_.dt);
+ true_X = Update(true_X, U, false);
+ ekf_.RawUpdateEncodersAndGyro(true_X(StateIdx::kLeftEncoder, 0),
+ true_X(StateIdx::kRightEncoder, 0),
+ (true_X(StateIdx::kRightVelocity, 0) -
+ true_X(StateIdx::kLeftVelocity, 0)) /
+ dt_config_.robot_radius / 2.0,
+ U, t0_ + (ii + 1) * dt_config_.dt);
EXPECT_NEAR((true_X - ekf_.X_hat()).squaredNorm(), 0.0, 1e-25)
<< "Expected only floating point precision errors in update step. "
"Estimated X_hat:\n"
@@ -303,29 +406,6 @@
}
}
-// Tests that encoder updates cause everything to converge properly in the
-// presence of voltage error.
-TEST_F(HybridEkfTest, PerfectEncoderUpdatesWithVoltageError) {
- State true_X = ekf_.X_hat();
- true_X(StateIdx::kLeftVoltageError, 0) = 2.0;
- true_X(StateIdx::kRightVoltageError, 0) = 2.0;
- Input U(5.0, 5.0);
- for (int ii = 0; ii < 1000; ++ii) {
- true_X = Update(true_X, U);
- ekf_.UpdateEncodersAndGyro(true_X(StateIdx::kLeftEncoder, 0),
- true_X(StateIdx::kRightEncoder, 0),
- (true_X(StateIdx::kRightVelocity, 0) -
- true_X(StateIdx::kLeftVelocity, 0)) /
- dt_config_.robot_radius / 2.0,
- U, t0_ + (ii + 1) * dt_config_.dt);
- }
- EXPECT_NEAR((true_X - ekf_.X_hat()).norm(), 0.0, 5e-2)
- << "Expected non-x/y estimates to converge to correct. "
- "Estimated X_hat:\n"
- << ekf_.X_hat() << "\ntrue X:\n"
- << true_X;
-}
-
// Tests encoder/gyro updates when we have some errors in our estimate.
TEST_F(HybridEkfTest, PerfectEncoderUpdateConverges) {
// In order to simulate modelling errors, we add an angular_error and start
@@ -340,15 +420,16 @@
// Note: Because we don't have any absolute measurements used for corrections,
// we will get slightly off on the absolute x/y/theta, but the errors are so
// small they are negligible.
- Input U(10.0, 5.0);
+ Input U;
+ U << 10.0, 5.0, 0.0, 0.0;
for (int ii = 0; ii < 100; ++ii) {
- true_X = Update(true_X, U);
- ekf_.UpdateEncodersAndGyro(true_X(StateIdx::kLeftEncoder, 0),
- true_X(StateIdx::kRightEncoder, 0),
- (true_X(StateIdx::kRightVelocity, 0) -
- true_X(StateIdx::kLeftVelocity, 0)) /
- dt_config_.robot_radius / 2.0,
- U, t0_ + (ii + 1) * dt_config_.dt);
+ true_X = Update(true_X, U, false);
+ ekf_.RawUpdateEncodersAndGyro(true_X(StateIdx::kLeftEncoder, 0),
+ true_X(StateIdx::kRightEncoder, 0),
+ (true_X(StateIdx::kRightVelocity, 0) -
+ true_X(StateIdx::kLeftVelocity, 0)) /
+ dt_config_.robot_radius / 2.0,
+ U, t0_ + (ii + 1) * dt_config_.dt);
}
EXPECT_NEAR((true_X - ekf_.X_hat()).norm(), 0.0, 3e-5)
<< "Expected non-x/y estimates to converge to correct. "
@@ -365,10 +446,11 @@
true_X(StateIdx::kAngularError, 0) = 1.0;
true_X(StateIdx::kLeftEncoder, 0) += 2.0;
true_X(StateIdx::kRightEncoder, 0) -= 2.0;
- Input U(10.0, 5.0);
+ Input U;
+ U << 10.0, 5.0, 0.0, 0.0;
for (int ii = 0; ii < 100; ++ii) {
- true_X = Update(true_X, U);
- ekf_.UpdateEncodersAndGyro(
+ true_X = Update(true_X, U, false);
+ ekf_.RawUpdateEncodersAndGyro(
true_X(StateIdx::kLeftEncoder, 0) + Normal(1e-3),
true_X(StateIdx::kRightEncoder, 0) + Normal(1e-3),
(true_X(StateIdx::kRightVelocity, 0) -
@@ -391,15 +473,16 @@
TEST_F(HybridEkfDeathTest, DieIfUninitialized) {
HybridEkf<> ekf(dt_config_);
// Expect death if we fail to initialize before starting to provide updates.
- EXPECT_DEATH(ekf.UpdateEncodersAndGyro(0.0, 1.0, 2.0, {3.0, 3.0}, t0_),
- "observations_.empty()");
+ EXPECT_DEATH(
+ ekf.RawUpdateEncodersAndGyro(0.0, 1.0, 2.0, {3.0, 3.0, 0.0, 0.0}, t0_),
+ "observations_.empty()");
}
TEST_F(HybridEkfDeathTest, DieOnNoU) {
// Expect death if the user does not provide U when creating a fresh
// measurement.
EXPECT_DEATH(ekf_.Correct({1, 2, 3}, nullptr, {}, {}, {}, {},
- t0_ + ::std::chrono::seconds(1)),
+ t0_ + std::chrono::seconds(1)),
"U != nullptr");
}
@@ -407,22 +490,23 @@
// that we die when an improper combination is provided.
TEST_F(HybridEkfDeathTest, DieOnNoH) {
// Check that we die when no h-related functions are provided:
- Input U(1, 2);
+ Input U;
+ U << 1.0, 2.0, 0.0, 0.0;
EXPECT_DEATH(ekf_.Correct({1, 2, 3}, &U, {}, {}, {}, {},
- t0_ + ::std::chrono::seconds(1)),
+ t0_ + std::chrono::seconds(1)),
"make_h");
// Check that we die when only one of h and dhdx are provided:
EXPECT_DEATH(ekf_.Correct({1, 2, 3}, &U, {}, {},
[](const State &) {
- return Eigen::Matrix<double, 3, 10>::Zero();
+ return Eigen::Matrix<double, 3, 12>::Zero();
},
- {}, t0_ + ::std::chrono::seconds(1)),
+ {}, t0_ + std::chrono::seconds(1)),
"make_h");
EXPECT_DEATH(ekf_.Correct({1, 2, 3}, &U, {},
[](const State &, const Input &) {
return Eigen::Matrix<double, 3, 1>::Zero();
},
- {}, {}, t0_ + ::std::chrono::seconds(1)),
+ {}, {}, t0_ + std::chrono::seconds(1)),
"make_h");
}
diff --git a/frc971/control_loops/drivetrain/libspline.cc b/frc971/control_loops/drivetrain/libspline.cc
index f0b9537..5ad175e 100644
--- a/frc971/control_loops/drivetrain/libspline.cc
+++ b/frc971/control_loops/drivetrain/libspline.cc
@@ -161,7 +161,7 @@
// This assumes that res is created in python to be getPathLength() long.
// Likely to SEGFAULT otherwise.
- void Distances(Trajectory *t, double *res) {
+ void TrajectoryDistances(Trajectory *t, double *res) {
const ::std::vector<double> &distances = t->Distances();
::std::memcpy(res, distances.data(), sizeof(double) * distances.size());
}
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index 2589d4b..6677145 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -3,6 +3,7 @@
#include "aos/events/event_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
#include "frc971/control_loops/pose.h"
@@ -36,6 +37,11 @@
// Defines an interface for classes that provide field-global localization.
class LocalizerInterface {
public:
+ typedef HybridEkf<double> Ekf;
+ typedef typename Ekf::StateIdx StateIdx;
+
+ virtual ~LocalizerInterface() {}
+
// Perform a single step of the filter, using the information that is
// available on every drivetrain iteration.
// The user should pass in the U that the real system experienced from the
@@ -48,23 +54,51 @@
virtual void Update(const ::Eigen::Matrix<double, 2, 1> &U,
::aos::monotonic_clock::time_point now,
double left_encoder, double right_encoder,
- double gyro_rate, double longitudinal_accelerometer) = 0;
+ double gyro_rate, const Eigen::Vector3d &accel) = 0;
// Reset the absolute position of the estimator.
virtual void ResetPosition(::aos::monotonic_clock::time_point t, double x,
double y, double theta, double theta_uncertainty,
bool reset_theta) = 0;
+ flatbuffers::Offset<LocalizerState> PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ LocalizerState::Builder builder(*fbb);
+ builder.add_x(x());
+ builder.add_y(y());
+ builder.add_theta(theta());
+ builder.add_left_velocity(left_velocity());
+ builder.add_right_velocity(right_velocity());
+ builder.add_left_encoder(left_encoder());
+ builder.add_right_encoder(right_encoder());
+ builder.add_left_voltage_error(left_voltage_error());
+ builder.add_right_voltage_error(right_voltage_error());
+ builder.add_angular_error(angular_error());
+ builder.add_longitudinal_velocity_offset(longitudinal_velocity_offset());
+ builder.add_lateral_velocity(lateral_velocity());
+ return builder.Finish();
+ }
+ virtual Ekf::State Xhat() const = 0;
// There are several subtly different norms floating around for state
- // matrices. In order to avoid that mess, we jus tprovide direct accessors for
+ // matrices. In order to avoid that mess, we just provide direct accessors for
// the values that most people care about.
- virtual double x() const = 0;
- virtual double y() const = 0;
- virtual double theta() const = 0;
- virtual double left_velocity() const = 0;
- virtual double right_velocity() const = 0;
- virtual double left_encoder() const = 0;
- virtual double right_encoder() const = 0;
- virtual double left_voltage_error() const = 0;
- virtual double right_voltage_error() const = 0;
+ double x() const { return Xhat()(StateIdx::kX); }
+ double y() const { return Xhat()(StateIdx::kY); }
+ double theta() const { return Xhat()(StateIdx::kTheta); }
+ double left_velocity() const { return Xhat()(StateIdx::kLeftVelocity); }
+ double right_velocity() const { return Xhat()(StateIdx::kRightVelocity); }
+ double left_encoder() const { return Xhat()(StateIdx::kLeftEncoder); }
+ double right_encoder() const { return Xhat()(StateIdx::kRightEncoder); }
+ double left_voltage_error() const {
+ return Xhat()(StateIdx::kLeftVoltageError);
+ }
+ double right_voltage_error() const {
+ return Xhat()(StateIdx::kRightVoltageError);
+ }
+ double angular_error() const { return Xhat()(StateIdx::kAngularError); }
+ double longitudinal_velocity_offset() const {
+ return Xhat()(StateIdx::kLongitudinalVelocityOffset);
+ }
+ double lateral_velocity() const { return Xhat()(StateIdx::kLateralVelocity); }
+
virtual TargetSelectorInterface *target_selector() = 0;
};
@@ -93,9 +127,6 @@
// This provides no method for using cameras or the such to get global
// measurements and just assumes that you can dead-reckon perfectly.
class DeadReckonEkf : public LocalizerInterface {
- typedef HybridEkf<double> Ekf;
- typedef typename Ekf::StateIdx StateIdx;
-
public:
DeadReckonEkf(::aos::EventLoop *event_loop,
const DrivetrainConfig<double> &dt_config)
@@ -107,11 +138,14 @@
target_selector_.set_has_target(false);
}
+ virtual ~DeadReckonEkf() {}
+
void Update(const ::Eigen::Matrix<double, 2, 1> &U,
::aos::monotonic_clock::time_point now, double left_encoder,
double right_encoder, double gyro_rate,
- double /*longitudinal_accelerometer*/) override {
- ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U, now);
+ const Eigen::Vector3d &accel) override {
+ ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U, accel,
+ now);
}
void ResetPosition(::aos::monotonic_clock::time_point t, double x, double y,
@@ -119,33 +153,14 @@
bool /*reset_theta*/) override {
const double left_encoder = ekf_.X_hat(StateIdx::kLeftEncoder);
const double right_encoder = ekf_.X_hat(StateIdx::kRightEncoder);
- ekf_.ResetInitialState(t, (Ekf::State() << x, y, theta, left_encoder, 0,
- right_encoder, 0, 0, 0,
- 0).finished(),
+ ekf_.ResetInitialState(t,
+ (Ekf::State() << x, y, theta, left_encoder, 0,
+ right_encoder, 0, 0, 0, 0, 0, 0)
+ .finished(),
ekf_.P());
- };
+ }
- double x() const override { return ekf_.X_hat(StateIdx::kX); }
- double y() const override { return ekf_.X_hat(StateIdx::kY); }
- double theta() const override { return ekf_.X_hat(StateIdx::kTheta); }
- double left_encoder() const override {
- return ekf_.X_hat(StateIdx::kLeftEncoder);
- }
- double right_encoder() const override {
- return ekf_.X_hat(StateIdx::kRightEncoder);
- }
- double left_velocity() const override {
- return ekf_.X_hat(StateIdx::kLeftVelocity);
- }
- double right_velocity() const override {
- return ekf_.X_hat(StateIdx::kRightVelocity);
- }
- double left_voltage_error() const override {
- return ekf_.X_hat(StateIdx::kLeftVoltageError);
- }
- double right_voltage_error() const override {
- return ekf_.X_hat(StateIdx::kRightVoltageError);
- }
+ Ekf::State Xhat() const override { return ekf_.X_hat(); }
TrivialTargetSelector *target_selector() override {
return &target_selector_;
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index b9ec5c5..439f14f 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -229,7 +229,7 @@
double default_acceleration)
: ProfiledSubsystem<3, 1, ZeroingEstimator>(
::std::move(loop), {{ZeroingEstimator(zeroing_constants)}}),
- profile_(::aos::controls::kLoopFrequency),
+ profile_(this->loop_->plant().coefficients().dt),
range_(range),
default_velocity_(default_velocity),
default_acceleration_(default_acceleration) {
@@ -275,7 +275,7 @@
builder.add_voltage_error(this->X_hat(2, 0));
builder.add_calculated_velocity(
(position() - last_position_) /
- ::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency));
+ ::aos::time::DurationInSeconds(this->loop_->plant().coefficients().dt));
builder.add_estimator_state(estimator_state);
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index 50764c7..1fae02b 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -164,18 +164,25 @@
)
py_binary(
- name = "path_edit",
+ name = "spline_graph",
srcs = [
- "basic_window.py",
"color.py",
+ "spline_drawing.py",
+ "spline_writer.py",
"path_edit.py",
+ "points.py",
+ "graph.py",
+ "spline_graph.py",
],
+ legacy_create_init = False,
restricted_to = ["//tools:k8"],
visibility = ["//visibility:public"],
deps = [
":libspline",
":python_init",
"@python_gtk",
+ "@matplotlib_repo//:matplotlib2.7",
+ ":basic_window",
],
)
diff --git a/frc971/control_loops/python/basic_window.py b/frc971/control_loops/python/basic_window.py
old mode 100644
new mode 100755
index 78324a3..827fe64
--- a/frc971/control_loops/python/basic_window.py
+++ b/frc971/control_loops/python/basic_window.py
@@ -1,3 +1,4 @@
+#!/usr/bin/python3
import gi
gi.require_version('Gtk', '3.0')
from gi.repository import Gtk
@@ -5,10 +6,10 @@
from gi.repository import Gdk
from gi.repository import GdkX11
import cairo
+from constants import *
identity = cairo.Matrix()
-
# Override the matrix of a cairo context.
class OverrideMatrix(object):
def __init__(self, cr, matrix):
@@ -34,7 +35,6 @@
def quit_main_loop(*args):
mainloop.quit()
-
def RunApp():
try:
mainloop.run()
@@ -49,11 +49,10 @@
# Draw in response to an expose-event
def __init__(self):
super(BaseWindow, self).__init__()
- #self.window.connect("destroy", quit_main_loop)
- self.set_size_request(640, 600)
+ self.set_size_request(2*SCREEN_SIZE, SCREEN_SIZE)
self.center = (0, 0)
- self.shape = (640, 400)
+ self.shape = (2*SCREEN_SIZE, SCREEN_SIZE)
self.needs_redraw = False
def get_current_scale(self):
@@ -81,3 +80,4 @@
# Handle the expose-event by drawing
def handle_draw(self, cr):
pass
+
diff --git a/frc971/control_loops/python/color.py b/frc971/control_loops/python/color.py
index ec53e82..5634042 100644
--- a/frc971/control_loops/python/color.py
+++ b/frc971/control_loops/python/color.py
@@ -1,9 +1,10 @@
class Color:
def __init__(self, r, g, b, a=1.0):
- self.r = r
- self.g = g
- self.b = b
- self.a = a
+ self.r = r
+ self.g = g
+ self.b = b
+ self.a = a
+
palette = {
"RED": Color(1, 0, 0),
@@ -16,5 +17,6 @@
"WHITE": Color(1, 1, 1),
"GREY": Color(0.5, 0.5, 0.5),
"LIGHT_GREY": Color(0.75, 0.75, 0.75),
- "DARK_GREY": Color(0.25, 0.25, 0.25)
+ "DARK_GREY": Color(0.25, 0.25, 0.25),
+ "ORANGE": Color(1, 0.65, 0)
}
diff --git a/frc971/control_loops/python/constants.py b/frc971/control_loops/python/constants.py
new file mode 100644
index 0000000..7b45215
--- /dev/null
+++ b/frc971/control_loops/python/constants.py
@@ -0,0 +1,28 @@
+import argparse
+
+arg_parser = argparse.ArgumentParser(description='spline_editor')
+arg_parser.add_argument(
+ 'size',
+ metavar='N',
+ default=800,
+ type=int,
+ nargs='?',
+ help="size of the screen")
+args = arg_parser.parse_args()
+SCREEN_SIZE = args.size
+
+WIDTH_OF_FIELD_IN_METERS = 8.258302
+
+WIDTH_OF_ROBOT = 0.65
+LENGTH_OF_ROBOT = 0.8
+
+ROBOT_SIDE_TO_BALL_CENTER = 0.15 #Placeholder value
+BALL_RADIUS = 0.165
+ROBOT_SIDE_TO_HATCH_PANEL = 0.1 #Placeholder value
+HATCH_PANEL_WIDTH = 0.4826
+
+def pxToM(p):
+ return p * WIDTH_OF_FIELD_IN_METERS / SCREEN_SIZE
+
+def mToPx(m):
+ return (m*SCREEN_SIZE/WIDTH_OF_FIELD_IN_METERS)
diff --git a/frc971/control_loops/python/drawing_constants.py b/frc971/control_loops/python/drawing_constants.py
new file mode 100644
index 0000000..89979f0
--- /dev/null
+++ b/frc971/control_loops/python/drawing_constants.py
@@ -0,0 +1,314 @@
+import cairo
+from color import Color, palette
+from constants import *
+import numpy as np
+
+
+def set_color(cr, color, a=1):
+ if color.a == 1.0:
+ cr.set_source_rgba(color.r, color.g, color.b, a)
+ else:
+ cr.set_source_rgba(color.r, color.g, color.b, color.a)
+
+
+def draw_px_cross(cr, x, y, length_px, color=palette["RED"]):
+ """Draws a cross with fixed dimensions in pixel space."""
+ set_color(cr, color)
+ cr.move_to(x, y - length_px)
+ cr.line_to(x, y + length_px)
+ cr.stroke()
+
+ cr.move_to(x - length_px, y)
+ cr.line_to(x + length_px, y)
+ cr.stroke()
+ set_color(cr, palette["WHITE"])
+
+
+def draw_px_x(cr, x, y, length_px1, color=palette["BLACK"]):
+ """Draws a x with fixed dimensions in pixel space."""
+ length_px = length_px1 / np.sqrt(2)
+ set_color(cr, color)
+ cr.move_to(x - length_px, y - length_px)
+ cr.line_to(x + length_px, y + length_px)
+ cr.stroke()
+
+ cr.move_to(x - length_px, y + length_px)
+ cr.line_to(x + length_px, y - length_px)
+ cr.stroke()
+ set_color(cr, palette["WHITE"])
+
+
+def draw_control_points(cr, points, width=10, radius=4, color=palette["BLUE"]):
+ for i in range(0, len(points)):
+ draw_px_x(cr, points[i][0], points[i][1], width, color)
+ set_color(cr, color)
+ cr.arc(points[i][0], points[i][1], radius, 0, 2.0 * np.pi)
+ cr.fill()
+ set_color(cr, palette["WHITE"])
+
+
+def display_text(cr, text, widtha, heighta, widthb, heightb):
+ cr.scale(widtha, -heighta)
+ cr.show_text(text)
+ cr.scale(widthb, -heightb)
+
+
+def draw_HAB(cr):
+ # BASE Constants
+ X_BASE = 0
+ Y_BASE = 0
+ R = 0.381 - .1
+ BACKWALL_X = X_BASE
+ LOADING_Y = mToPx(4.129151) - mToPx(0.696976)
+ # HAB Levels 2 and 3 called in variables backhab
+ # draw loading stations
+ cr.move_to(0, LOADING_Y)
+ cr.line_to(mToPx(0.6), LOADING_Y)
+ cr.move_to(mToPx(R), LOADING_Y)
+ cr.arc(mToPx(R), LOADING_Y, 5, 0, np.pi * 2.0)
+ cr.move_to(0, -1.0 * LOADING_Y)
+ cr.line_to(mToPx(0.6), -1.0 * LOADING_Y)
+ cr.move_to(mToPx(R), -1.0 * LOADING_Y)
+ cr.arc(mToPx(R), -1.0 * LOADING_Y, 5, 0, np.pi * 2.0)
+
+ # HAB Levels 2 and 3 called in variables backhab
+ WIDTH_BACKHAB = mToPx(1.2192)
+
+ Y_TOP_BACKHAB_BOX = Y_BASE + mToPx(0.6096)
+ BACKHAB_LV2_LENGTH = mToPx(1.016)
+
+ BACKHAB_LV3_LENGTH = mToPx(1.2192)
+ Y_LV3_BOX = Y_TOP_BACKHAB_BOX - BACKHAB_LV3_LENGTH
+
+ Y_BOTTOM_BACKHAB_BOX = Y_LV3_BOX - BACKHAB_LV2_LENGTH
+
+ # HAB LEVEL 1
+ X_LV1_BOX = BACKWALL_X + WIDTH_BACKHAB
+
+ WIDTH_LV1_BOX = mToPx(0.90805)
+ LENGTH_LV1_BOX = mToPx(1.6256)
+
+ Y_BOTTOM_LV1_BOX = Y_BASE - LENGTH_LV1_BOX
+
+ # Ramp off Level 1
+ X_RAMP = X_LV1_BOX
+
+ Y_TOP_RAMP = Y_BASE + LENGTH_LV1_BOX
+ WIDTH_TOP_RAMP = mToPx(1.20015)
+ LENGTH_TOP_RAMP = Y_BASE + mToPx(0.28306)
+
+ X_MIDDLE_RAMP = X_RAMP + WIDTH_LV1_BOX
+ Y_MIDDLE_RAMP = Y_BOTTOM_LV1_BOX
+ LENGTH_MIDDLE_RAMP = 2 * LENGTH_LV1_BOX
+ WIDTH_MIDDLE_RAMP = WIDTH_TOP_RAMP - WIDTH_LV1_BOX
+
+ Y_BOTTOM_RAMP = Y_BASE - LENGTH_LV1_BOX - LENGTH_TOP_RAMP
+
+ # Side Bars to Hold in balls
+ X_BARS = BACKWALL_X
+ WIDTH_BARS = WIDTH_BACKHAB
+ LENGTH_BARS = mToPx(0.574675)
+
+ Y_TOP_BAR = Y_TOP_BACKHAB_BOX + BACKHAB_LV2_LENGTH
+
+ Y_BOTTOM_BAR = Y_BOTTOM_BACKHAB_BOX - LENGTH_BARS
+
+ set_color(cr, palette["BLACK"])
+ cr.rectangle(BACKWALL_X, Y_TOP_BACKHAB_BOX, WIDTH_BACKHAB,
+ BACKHAB_LV2_LENGTH)
+ cr.rectangle(BACKWALL_X, Y_LV3_BOX, WIDTH_BACKHAB, BACKHAB_LV3_LENGTH)
+ cr.rectangle(BACKWALL_X, Y_BOTTOM_BACKHAB_BOX, WIDTH_BACKHAB,
+ BACKHAB_LV2_LENGTH)
+ cr.rectangle(X_LV1_BOX, Y_BASE, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
+ cr.rectangle(X_LV1_BOX, Y_BOTTOM_LV1_BOX, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
+ cr.rectangle(X_RAMP, Y_TOP_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
+ cr.rectangle(X_MIDDLE_RAMP, Y_MIDDLE_RAMP, WIDTH_MIDDLE_RAMP,
+ LENGTH_MIDDLE_RAMP)
+ cr.rectangle(X_RAMP, Y_BOTTOM_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
+ cr.rectangle(X_BARS, Y_TOP_BAR, WIDTH_BARS, LENGTH_BARS)
+ cr.rectangle(X_BARS, Y_BOTTOM_BAR, WIDTH_BARS, LENGTH_BARS)
+ cr.stroke()
+
+ cr.set_line_join(cairo.LINE_JOIN_ROUND)
+
+ cr.stroke()
+
+ #draw 0, 0
+ set_color(cr, palette["BLACK"])
+ cr.move_to(0, 0)
+ cr.line_to(0, 0 + mToPx(8.2296 / 2.0))
+ cr.move_to(0, 0)
+ cr.line_to(0, 0 + mToPx(-8.2296 / 2.0))
+ cr.move_to(0, 0)
+ cr.line_to(0 + mToPx(8.2296), 0)
+
+ cr.stroke()
+
+
+def draw_rockets(cr):
+ # BASE Constants
+ X_BASE = mToPx(2.41568)
+ Y_BASE = 0
+ # Robot longitudinal radius
+ R = 0.381
+ near_side_rocket_center = [
+ X_BASE + mToPx((2.89973 + 3.15642) / 2.0), Y_BASE + mToPx(
+ (3.86305 + 3.39548) / 2.0)
+ ]
+ middle_rocket_center = [
+ X_BASE + mToPx((3.15642 + 3.6347) / 2.0), Y_BASE + mToPx(
+ (3.39548 + 3.392380) / 2.0)
+ ]
+ far_side_rocket_center = [
+ X_BASE + mToPx((3.63473 + 3.89984) / 2.0), Y_BASE + mToPx(
+ (3.39238 + 3.86305) / 2.0)
+ ]
+
+ cr.move_to(near_side_rocket_center[0], near_side_rocket_center[1])
+ cr.line_to(near_side_rocket_center[0] - 0.8 * mToPx(0.866),
+ near_side_rocket_center[1] - 0.8 * mToPx(0.5))
+ cr.move_to(near_side_rocket_center[0] - R * mToPx(0.866),
+ near_side_rocket_center[1] - R * mToPx(0.5))
+ cr.arc(near_side_rocket_center[0] - R * mToPx(0.866),
+ near_side_rocket_center[1] - R * mToPx(0.5), 5, 0, np.pi * 2.0)
+
+ cr.move_to(middle_rocket_center[0], middle_rocket_center[1])
+ cr.line_to(middle_rocket_center[0], middle_rocket_center[1] - mToPx(0.8))
+ cr.move_to(middle_rocket_center[0], middle_rocket_center[1] - mToPx(R))
+ cr.arc(middle_rocket_center[0], middle_rocket_center[1] - mToPx(R), 5, 0,
+ np.pi * 2.0)
+
+ cr.move_to(far_side_rocket_center[0], far_side_rocket_center[1])
+ cr.line_to(far_side_rocket_center[0] + 0.8 * mToPx(0.866),
+ far_side_rocket_center[1] - 0.8 * mToPx(0.5))
+ cr.move_to(far_side_rocket_center[0] + R * mToPx(0.866),
+ far_side_rocket_center[1] - R * mToPx(0.5))
+ cr.arc(far_side_rocket_center[0] + R * mToPx(0.866),
+ far_side_rocket_center[1] - R * mToPx(0.5), 5, 0, np.pi * 2.0)
+
+ #print(far_side_rocket_center)
+ near_side_rocket_center = [
+ X_BASE + mToPx((2.89973 + 3.15642) / 2.0), Y_BASE - mToPx(
+ (3.86305 + 3.39548) / 2.0)
+ ]
+ middle_rocket_center = [
+ X_BASE + mToPx((3.15642 + 3.6347) / 2.0), Y_BASE - mToPx(
+ (3.39548 + 3.392380) / 2.0)
+ ]
+ far_side_rocket_center = [
+ X_BASE + mToPx((3.63473 + 3.89984) / 2.0), Y_BASE - mToPx(
+ (3.39238 + 3.86305) / 2.0)
+ ]
+
+ cr.move_to(near_side_rocket_center[0], near_side_rocket_center[1])
+ cr.line_to(near_side_rocket_center[0] - 0.8 * mToPx(0.866),
+ near_side_rocket_center[1] + 0.8 * mToPx(0.5))
+
+ cr.move_to(middle_rocket_center[0], middle_rocket_center[1])
+ cr.line_to(middle_rocket_center[0], middle_rocket_center[1] + mToPx(0.8))
+
+ cr.move_to(far_side_rocket_center[0], far_side_rocket_center[1])
+ cr.line_to(far_side_rocket_center[0] + 0.8 * mToPx(0.866),
+ far_side_rocket_center[1] + 0.8 * mToPx(0.5))
+
+ # Leftmost Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
+
+ # Top Line
+ cr.move_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
+ cr.line_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
+
+ #Rightmost Line
+ cr.move_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
+
+ #Back Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
+
+ # Bottom Rocket
+ # Leftmost Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
+
+ # Top Line
+ cr.move_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
+ cr.line_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
+
+ #Rightmost Line
+ cr.move_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
+
+ #Back Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
+
+ cr.stroke()
+
+
+def draw_cargo_ship(cr):
+ # BASE Constants
+ X_BASE = 0 + mToPx(5.59435)
+ Y_BASE = 0 + 0 #mToPx(4.129151)
+ R = 0.381 - 0.1
+
+ FRONT_PEG_DELTA_Y = mToPx(0.276352)
+ cr.move_to(X_BASE, Y_BASE + FRONT_PEG_DELTA_Y)
+ cr.line_to(X_BASE - mToPx(0.8), Y_BASE + FRONT_PEG_DELTA_Y)
+
+ cr.move_to(X_BASE, Y_BASE + FRONT_PEG_DELTA_Y)
+ cr.arc(X_BASE - mToPx(R), Y_BASE + FRONT_PEG_DELTA_Y, 5, 0, np.pi * 2.0)
+
+ cr.move_to(X_BASE, Y_BASE - FRONT_PEG_DELTA_Y)
+ cr.line_to(X_BASE - mToPx(0.8), Y_BASE - FRONT_PEG_DELTA_Y)
+
+ cr.move_to(X_BASE, Y_BASE - FRONT_PEG_DELTA_Y)
+ cr.arc(X_BASE - mToPx(R), Y_BASE - FRONT_PEG_DELTA_Y, 5, 0, np.pi * 2.0)
+
+ SIDE_PEG_Y = mToPx(1.41605 / 2.0)
+ SIDE_PEG_X = X_BASE + mToPx(1.148842)
+ SIDE_PEG_DX = mToPx(0.55245)
+
+ cr.move_to(SIDE_PEG_X, SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X, SIDE_PEG_Y + mToPx(0.8))
+ cr.move_to(SIDE_PEG_X, SIDE_PEG_Y + mToPx(R))
+ cr.arc(SIDE_PEG_X, SIDE_PEG_Y + mToPx(R), 5, 0, np.pi * 2.0)
+
+ cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y + mToPx(0.8))
+ cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y + mToPx(R))
+ cr.arc(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y + mToPx(R), 5, 0, np.pi * 2.0)
+
+ cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y + mToPx(0.8))
+ cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y + mToPx(R))
+ cr.arc(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y + mToPx(R), 5, 0,
+ np.pi * 2.0)
+
+ cr.move_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y - mToPx(0.8))
+ cr.move_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y - mToPx(R))
+ cr.arc(SIDE_PEG_X, -1.0 * SIDE_PEG_Y - mToPx(R), 5, 0, np.pi * 2.0)
+
+ cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(0.8))
+ cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(R))
+ cr.arc(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(R), 5, 0,
+ np.pi * 2.0)
+
+ cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(0.8))
+ cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(R))
+ cr.arc(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(R), 5, 0,
+ np.pi * 2.0)
+
+ cr.rectangle(X_BASE, Y_BASE - mToPx(1.41605 / 2.0), mToPx(2.43205),
+ mToPx(1.41605))
+ cr.stroke()
+
+
+def draw_points(cr, p, size):
+ for i in range(0, len(p)):
+ draw_px_cross(cr, p[i][0], p[i][1], size,
+ Color(0, np.sqrt(0.2 * i), 0))
diff --git a/frc971/control_loops/python/graph.py b/frc971/control_loops/python/graph.py
new file mode 100644
index 0000000..7e98a38
--- /dev/null
+++ b/frc971/control_loops/python/graph.py
@@ -0,0 +1,178 @@
+from constants import *
+import cairo
+from color import Color, palette
+from points import Points
+from drawing_constants import *
+from libspline import Spline, DistanceSpline, Trajectory
+
+AXIS_MARGIN_SPACE = 40
+
+
+class Graph(): # (TODO): Remove Computer Calculation
+ def __init__(self, cr, mypoints):
+ # Background Box
+ set_color(cr, palette["WHITE"])
+ cr.rectangle(-1.0 * SCREEN_SIZE, -0.5 * SCREEN_SIZE, SCREEN_SIZE,
+ SCREEN_SIZE * 0.6)
+ cr.fill()
+
+ cr.set_source_rgb(0, 0, 0)
+ cr.rectangle(-1.0 * SCREEN_SIZE, -0.5 * SCREEN_SIZE, SCREEN_SIZE,
+ SCREEN_SIZE * 0.6)
+ #Axis
+ cr.move_to(-1.0 * SCREEN_SIZE + AXIS_MARGIN_SPACE,
+ -0.5 * SCREEN_SIZE + AXIS_MARGIN_SPACE) # Y
+ cr.line_to(-1.0 * SCREEN_SIZE + AXIS_MARGIN_SPACE,
+ 0.1 * SCREEN_SIZE - 10)
+
+ cr.move_to(-1.0 * SCREEN_SIZE + AXIS_MARGIN_SPACE,
+ -0.5 * SCREEN_SIZE + AXIS_MARGIN_SPACE) # X
+ cr.line_to(-10, -0.5 * SCREEN_SIZE + AXIS_MARGIN_SPACE)
+ cr.stroke()
+
+ skip = 2
+ dT = 0.00505
+ start = AXIS_MARGIN_SPACE - SCREEN_SIZE
+ end = -2.0 * AXIS_MARGIN_SPACE
+ height = 0.5 * (SCREEN_SIZE) - AXIS_MARGIN_SPACE
+ zero = AXIS_MARGIN_SPACE - SCREEN_SIZE / 2.0
+ if mypoints.getLibsplines():
+ distanceSpline = DistanceSpline(mypoints.getLibsplines())
+ traj = Trajectory(distanceSpline)
+ traj.Plan()
+ XVA = traj.GetPlanXVA(dT)
+ self.draw_x_axis(cr, start, height, zero, XVA, end)
+ self.drawVelocity(cr, XVA, start, height, skip, zero, end)
+ self.drawAcceleration(cr, XVA, start, height, skip, zero,
+ AXIS_MARGIN_SPACE, end)
+ self.drawVoltage(cr, XVA, start, height, skip, traj, zero, end)
+ cr.set_source_rgb(0, 0, 0)
+ cr.move_to(-1.0 * AXIS_MARGIN_SPACE, zero + height / 2.0)
+ cr.line_to(AXIS_MARGIN_SPACE - SCREEN_SIZE, zero + height / 2.0)
+ cr.stroke()
+
+ def connectLines(self, cr, points, color):
+ for i in range(0, len(points) - 1):
+ set_color(cr, color)
+ cr.move_to(points[i][0], points[i][1])
+ cr.line_to(points[i + 1][0], points[i + 1][1])
+ cr.stroke()
+
+ def draw_x_axis(self, cr, start, height, zero, xva, end):
+ total_time = 0.00505 * len(xva[0])
+ for k in np.linspace(0, 1, 11):
+ self.tickMark(cr,
+ k * np.abs(start - end) + start, zero + height / 2.0,
+ 10, palette["BLACK"])
+ cr.move_to(k * np.abs(start - end) + start,
+ 10 + zero + height / 2.0)
+ txt_scale = SCREEN_SIZE / 1000.0
+ display_text(cr, str(round(k * total_time, 3)), txt_scale,
+ txt_scale, 1.0 / txt_scale, 1.0 / txt_scale)
+ cr.stroke()
+
+ def tickMark(self, cr, x, y, height, COLOR):
+ # X, Y is in the middle of the tick mark
+ set_color(cr, COLOR)
+ cr.move_to(x, y + (height / 2))
+ cr.line_to(x, y - (height / 2))
+ cr.stroke()
+
+ def HtickMark(self, cr, x, y, width, COLOR):
+ # X, Y is in the middle of the tick mark
+ set_color(cr, COLOR)
+ cr.move_to(x + (width / 2), y)
+ cr.line_to(x - (width / 2), y)
+ cr.stroke()
+
+ def drawVelocity(self, cr, xva, start, height, skip, zero, end):
+ COLOR = palette["RED"]
+ velocity = xva[1]
+ n_timesteps = len(velocity)
+ max_v = np.amax(velocity)
+ spacing = np.abs(start - end) / float(n_timesteps)
+ scaler = height / max_v
+ cr.set_source_rgb(1, 0, 0)
+ points = []
+ for i in range(0, len(velocity)):
+ if i % skip == 0:
+ points.append([
+ start + (i * spacing),
+ zero + height / 2.0 + (velocity[i] * scaler / 2.0)
+ ])
+ self.connectLines(cr, points, COLOR)
+
+ # draw axes marking
+ for i in np.linspace(-1, 1, 11):
+ self.HtickMark(cr, start, zero + i * height / 2.0 + height / 2.0,
+ 10, palette["BLACK"])
+ cr.set_source_rgb(1, 0, 0)
+ cr.move_to(start + 5, zero + i * height / 2.0 + height / 2.0)
+ txt_scale = SCREEN_SIZE / 1000.0
+ display_text(cr, str(round(i * max_v, 2)), txt_scale, txt_scale,
+ 1.0 / txt_scale, 1.0 / txt_scale)
+ cr.stroke()
+
+ def drawAcceleration(self, cr, xva, start, height, skip, zero, margin,
+ end):
+ COLOR = palette["BLUE"]
+ accel = xva[2]
+ max_a = np.amax(accel)
+ min_a = np.amin(accel)
+ n_timesteps = len(accel)
+ spacing = np.abs(start - end) / float(n_timesteps)
+ scaler = height / (max_a - min_a)
+ cr.set_source_rgb(1, 0, 0)
+ points = []
+ for i in range(0, len(accel)):
+ if i % skip == 0:
+ points.append([
+ start + (i * spacing), zero + ((accel[i] - min_a) * scaler)
+ ])
+ self.connectLines(cr, points, COLOR)
+
+ # draw axes marking
+ for i in np.linspace(0, 1, 11):
+ self.HtickMark(cr, -1.5 * margin, zero + i * height, 10,
+ palette["BLACK"])
+ cr.set_source_rgb(0, 0, 1)
+ cr.move_to(-1.2 * margin, zero + i * height)
+ txt_scale = SCREEN_SIZE / 1000.0
+ display_text(cr, str(round(i * (max_a - min_a) + min_a,
+ 2)), txt_scale, txt_scale,
+ 1.0 / txt_scale, 1.0 / txt_scale)
+ cr.stroke()
+
+ def drawVoltage(self, cr, xva, start, height, skip, traj, zero, end):
+ COLOR1 = palette["GREEN"]
+ COLOR2 = palette["CYAN"]
+ poses = xva[0]
+ n_timesteps = len(poses)
+ spacing = np.abs(start - end) / float(n_timesteps)
+ points1 = []
+ points2 = []
+ for i in range(0, len(poses)):
+ if i % skip == 0:
+ voltage = traj.Voltage(poses[i])
+ points1.append([
+ start + (i * spacing),
+ zero + height / 2 + height * (voltage[0] / 24.0)
+ ])
+ points2.append([
+ start + (i * spacing),
+ zero + height / 2 + height * (voltage[1] / 24.0)
+ ])
+ self.connectLines(cr, points1, COLOR1)
+ self.connectLines(cr, points2, COLOR2)
+
+ for i in np.linspace(-1, 1, 7):
+ self.HtickMark(cr, -1.0 * SCREEN_SIZE,
+ zero + i * height / 2.0 + height / 2.0, 10,
+ palette["BLACK"])
+ cr.set_source_rgb(0, 1, 1)
+ cr.move_to(-1.0 * SCREEN_SIZE,
+ zero + i * height / 2.0 + height / 2.0)
+ txt_scale = SCREEN_SIZE / 1000.0
+ display_text(cr, str(round(i * 12.0, 2)), txt_scale, txt_scale,
+ 1.0 / txt_scale, 1.0 / txt_scale)
+ cr.stroke()
diff --git a/frc971/control_loops/python/libspline.py b/frc971/control_loops/python/libspline.py
old mode 100644
new mode 100755
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
old mode 100644
new mode 100755
index e59286f..0d1b263
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -3,67 +3,23 @@
import os
import sys
import copy
-import basic_window
from color import Color, palette
import random
import gi
import numpy as np
-from libspline import Spline
import scipy.spatial.distance
gi.require_version('Gtk', '3.0')
+gi.require_version('Gdk', '3.0')
from gi.repository import Gdk, Gtk, GLib
import cairo
+from libspline import Spline, DistanceSpline, Trajectory
import enum
-import json # For writing to json files
-
-from basic_window import OverrideMatrix, identity, quit_main_loop, set_color
-
-WIDTH_OF_FIELD_IN_METERS = 8.258302
-PIXELS_ON_SCREEN = 300
-
-
-def pxToM(p):
- return p * WIDTH_OF_FIELD_IN_METERS / PIXELS_ON_SCREEN
-
-
-def mToPx(m):
- return (m*PIXELS_ON_SCREEN/WIDTH_OF_FIELD_IN_METERS)
-
-def px(cr):
- return OverrideMatrix(cr, identity)
-
-
-def draw_px_cross(cr, x, y, length_px, color=palette["RED"]):
- """Draws a cross with fixed dimensions in pixel space."""
- set_color(cr, color)
- cr.move_to(x, y - length_px)
- cr.line_to(x, y + length_px)
- cr.stroke()
-
- cr.move_to(x - length_px, y)
- cr.line_to(x + length_px, y)
- cr.stroke()
- set_color(cr, palette["LIGHT_GREY"])
-
-
-def draw_px_x(cr, x, y, length_px1, color=palette["BLACK"]):
- """Draws a x with fixed dimensions in pixel space."""
- length_px = length_px1 / np.sqrt(2)
- set_color(cr, color)
- cr.move_to(x - length_px, y - length_px)
- cr.line_to(x + length_px, y + length_px)
- cr.stroke()
-
- cr.move_to(x - length_px, y + length_px)
- cr.line_to(x + length_px, y - length_px)
- cr.stroke()
- set_color(cr, palette["LIGHT_GREY"])
-
-
-def draw_points(cr, p, size):
- for i in range(0, len(p)):
- draw_px_cross(cr, p[i][0], p[i][1], size, Color(
- 0, np.sqrt(0.2 * i), 0))
+import json
+from basic_window import *
+from constants import *
+from drawing_constants import *
+from points import Points
+from graph import Graph
class Mode(enum.Enum):
@@ -72,61 +28,25 @@
kEditing = 2
kExporting = 3
kImporting = 4
- kConstraint = 5
-class ConstraintType(enum.Enum):
- kMaxSpeed = 0
- kMaxAcceleration = 1
-
-
-def display_text(cr, text, widtha, heighta, widthb, heightb):
- cr.scale(widtha, -heighta)
- cr.show_text(text)
- cr.scale(widthb, -heightb)
-
-
-def redraw(needs_redraw, window):
- print("Redrew")
- if not needs_redraw:
- window.queue_draw()
-
-
-class Constraint():
- def __init__(self, start, end, constraint, value):
- self.start = start #Array with index and distance from start of spline
- self.end = end #Array with index and distance from start of spline
- self.constraint = constraint #INT
- self.value = value #INT
- if self.constraint == 0:
- self.conName = "kMaxSpeed"
- else:
- self.conName = "kMaxAcceleration"
-
- def toString(self):
-
- return "START: " + str(self.start[0]) + ", " + str(
- self.start[1]) + " | END: " + str(self.end[0]) + ", " + str(
- self.end[1]) + " | " + str(self.conName) + ": " + str(
- self.value)
-
-
-class GTK_Widget(basic_window.BaseWindow):
+class GTK_Widget(BaseWindow):
"""Create a GTK+ widget on which we will draw using Cairo"""
-
def __init__(self):
super(GTK_Widget, self).__init__()
+ self.points = Points()
+
# init field drawing
# add default spline for testing purposes
# init editing / viewing modes and pointer location
self.mode = Mode.kPlacing
self.x = 0
self.y = 0
-
module_path = os.path.dirname(os.path.realpath(sys.argv[0]))
self.path_to_export = os.path.join(module_path,
- 'points_for_pathedit.json')
+ 'points_for_pathedit.json')
+
# update list of control points
self.point_selected = False
# self.adding_spline = False
@@ -145,32 +65,20 @@
for c in palette:
self.colors.append(palette[c])
- self.selected_points = []
- self.splines = []
self.reinit_extents()
self.inStart = None
self.inEnd = None
- self.inConstraint = None
self.inValue = None
self.startSet = False
- #John also wrote this
- def add_point(self, x, y):
- if (len(self.selected_points) < 6):
- self.selected_points.append([pxToM(x), pxToM(y)])
- if (len(self.selected_points) == 6):
- self.mode = Mode.kEditing
- self.splines.append(np.array(self.selected_points))
- self.selected_points = []
-
"""set extents on images"""
def reinit_extents(self):
- self.extents_x_min = -800
- self.extents_x_max = 800
- self.extents_y_min = -800
- self.extents_y_max = 800
+ self.extents_x_min = -1.0 * SCREEN_SIZE
+ self.extents_x_max = SCREEN_SIZE
+ self.extents_y_min = -1.0 * SCREEN_SIZE
+ self.extents_y_max = SCREEN_SIZE
# this needs to be rewritten with numpy, i dont think this ought to have
# SciPy as a dependecy
@@ -184,192 +92,112 @@
def get_nearest_point(self):
return self.all_controls[self.get_index_of_nearest_point()]
- def set_index_to_nearest_spline_point(self):
- nearest = 50
- index_of_closest = 0
- self.spline_edit = 0
- cur_p = [self.x, self.y]
-
- for index_splines, points in enumerate(self.spline):
- for index_points, point in enumerate(points.curve):
- # pythagorean
- distance = np.sqrt((cur_p[0] - mToPx(point[0]))**2 +
- (cur_p[1] - mToPx(point[1])**2))
- if distance < nearest:
- nearest = distance
- print("DISTANCE: ", distance, " | INDEX: ", index_points)
- index_of_closest = index_points
- self.index_of_edit = index_of_closest
- self.spline_edit = index_splines
- self.held_x = self.x
- if self.startSet == False:
- self.inStart = [self.index_of_edit, self.findDistance()]
- self.startSet = True
- else:
- self.inEnd = [self.index_of_edit, self.findDistance()]
- self.startSet = False
- self.mode = Mode.kEditing
- self.spline_edit = -1
- self.index_of_edit = -1
-
- print("Nearest: " + str(nearest))
- print("Spline: " + str(self.spline_edit))
- print("Index: " + str(index_of_closest))
-
- def findDistance(self):
- """ findDistance goes through each point on the spline finding distance to that point from the point before.
- It does this to find the the length of the spline to the point that is currently selected.
- """
- distance = 0
- points = self.curves[self.spline_edit]
- for index, point in enumerate(points):
- if index > 0 and index <= self.index_of_edit:
- distance += np.sqrt((points[index - 1][0] - point[0])**2 +
- (points[index - 1][1] - point[1])**2)
- return distance
-
- def draw_HAB(self, cr):
- print("WENT IN")
- # BASE Constants
- X_BASE = -450+mToPx(2.41568)
- Y_BASE = -150+mToPx(4.129151)
-
- BACKWALL_X = -450
-
- # HAB Levels 2 and 3 called in variables backhab
-
- WIDTH_BACKHAB = mToPx(1.2192)
-
- Y_TOP_BACKHAB_BOX = Y_BASE + mToPx(0.6096)
- BACKHAB_LV2_LENGTH = mToPx(1.016)
-
- BACKHAB_LV3_LENGTH = mToPx(1.2192)
- Y_LV3_BOX = Y_TOP_BACKHAB_BOX - BACKHAB_LV3_LENGTH
-
- Y_BOTTOM_BACKHAB_BOX = Y_LV3_BOX -BACKHAB_LV2_LENGTH
-
- # HAB LEVEL 1
- X_LV1_BOX = BACKWALL_X + WIDTH_BACKHAB
-
- WIDTH_LV1_BOX = mToPx(0.90805)
- LENGTH_LV1_BOX = mToPx(1.6256)
-
- Y_BOTTOM_LV1_BOX = Y_BASE - LENGTH_LV1_BOX
-
- # Ramp off Level 1
- X_RAMP = X_LV1_BOX
-
- Y_TOP_RAMP = Y_BASE + LENGTH_LV1_BOX
- WIDTH_TOP_RAMP = mToPx(1.20015)
- LENGTH_TOP_RAMP = Y_BASE + mToPx(0.28306)
-
- X_MIDDLE_RAMP = X_RAMP + WIDTH_LV1_BOX
- Y_MIDDLE_RAMP = Y_BOTTOM_LV1_BOX
- LENGTH_MIDDLE_RAMP = 2*LENGTH_LV1_BOX
- WIDTH_MIDDLE_RAMP = WIDTH_TOP_RAMP - WIDTH_LV1_BOX
-
- Y_BOTTOM_RAMP = Y_BASE - LENGTH_LV1_BOX - LENGTH_TOP_RAMP
-
- # Side Bars to Hold in balls
- X_BARS = BACKWALL_X
- WIDTH_BARS = WIDTH_BACKHAB
- LENGTH_BARS = mToPx(0.574675)
-
- Y_TOP_BAR = Y_TOP_BACKHAB_BOX + BACKHAB_LV2_LENGTH
-
- Y_BOTTOM_BAR = Y_BOTTOM_BACKHAB_BOX - LENGTH_BARS
-
- set_color(cr, palette["BLACK"])
- cr.rectangle(BACKWALL_X, Y_TOP_BACKHAB_BOX, WIDTH_BACKHAB,
- BACKHAB_LV2_LENGTH)
- cr.rectangle(BACKWALL_X, Y_LV3_BOX, WIDTH_BACKHAB,
- BACKHAB_LV3_LENGTH)
- cr.rectangle(BACKWALL_X, Y_BOTTOM_BACKHAB_BOX, WIDTH_BACKHAB,
- BACKHAB_LV2_LENGTH)
- cr.rectangle(X_LV1_BOX, Y_BASE, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
- cr.rectangle(X_LV1_BOX, Y_BOTTOM_LV1_BOX, WIDTH_LV1_BOX,
- LENGTH_LV1_BOX)
- cr.rectangle(X_RAMP, Y_TOP_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
- cr.rectangle(X_MIDDLE_RAMP, Y_MIDDLE_RAMP, WIDTH_MIDDLE_RAMP,
- LENGTH_MIDDLE_RAMP)
- cr.rectangle(X_RAMP, Y_BOTTOM_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
- cr.rectangle(X_BARS, Y_TOP_BAR, WIDTH_BARS, LENGTH_BARS)
- cr.rectangle(X_BARS, Y_BOTTOM_BAR, WIDTH_BARS, LENGTH_BARS)
- cr.stroke()
- #draw_px_x(cr, BACKWALL_X, 0, 10) # Midline Point
- #draw_px_x(cr, X_BASE, Y_BASE, 10) # Bases
- cr.set_line_join(cairo.LINE_JOIN_ROUND)
-
- cr.stroke()
-
- def draw_rockets(self, cr):
- # BASE Constants
- X_BASE = -450+mToPx(2.41568)
- Y_BASE = -150+mToPx(4.129151)
-
- # Top Rocket
-
- # Leftmost Line
- cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
- cr.line_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
-
- # Top Line
- cr.move_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
- cr.line_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
-
- #Rightmost Line
- cr.move_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
- cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
-
- #Back Line
- cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
- cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
-
- # Bottom Rocket
-
- # Leftmost Line
- cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
- cr.line_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
-
- # Top Line
- cr.move_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
- cr.line_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
-
- #Rightmost Line
- cr.move_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
- cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
-
- #Back Line
- cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
- cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
-
- cr.stroke()
-
- def draw_cargo_ship(self, cr):
- # BASE Constants
- X_BASE = -450+mToPx(2.41568)
- Y_BASE = -150+mToPx(4.129151)
-
- cr.rectangle(X_BASE + mToPx(3.15912), Y_BASE - mToPx(0.72326),
- mToPx(2.43205), mToPx(1.41605))
-
- cr.stroke()
-
def draw_field_elements(self, cr):
- self.draw_HAB(cr)
- self.draw_rockets(cr)
- self.draw_cargo_ship(cr)
+ draw_HAB(cr)
+ draw_rockets(cr)
+ draw_cargo_ship(cr)
- def handle_draw(self, cr):
- # print(self.new_point)
- # print("SELF.POINT_SELECTED: " + str(self.point_selected))
+ def draw_robot_at_point(self, cr, i, p, spline):
+ p1 = [mToPx(spline.Point(i)[0]), mToPx(spline.Point(i)[1])]
+ p2 = [mToPx(spline.Point(i + p)[0]), mToPx(spline.Point(i + p)[1])]
- # begin drawing
+ #Calculate Robot
+ distance = np.sqrt((p2[1] - p1[1])**2 + (p2[0] - p1[0])**2)
+ x_difference_o = p2[0] - p1[0]
+ y_difference_o = p2[1] - p1[1]
+ x_difference = x_difference_o * mToPx(LENGTH_OF_ROBOT / 2) / distance
+ y_difference = y_difference_o * mToPx(LENGTH_OF_ROBOT / 2) / distance
+
+ front_middle = []
+ front_middle.append(p1[0] + x_difference)
+ front_middle.append(p1[1] + y_difference)
+
+ back_middle = []
+ back_middle.append(p1[0] - x_difference)
+ back_middle.append(p1[1] - y_difference)
+
+ slope = [-(1 / x_difference_o) / (1 / y_difference_o)]
+ angle = np.arctan(slope)
+
+ x_difference = np.sin(angle[0]) * mToPx(WIDTH_OF_ROBOT / 2)
+ y_difference = np.cos(angle[0]) * mToPx(WIDTH_OF_ROBOT / 2)
+
+ front_1 = []
+ front_1.append(front_middle[0] - x_difference)
+ front_1.append(front_middle[1] - y_difference)
+
+ front_2 = []
+ front_2.append(front_middle[0] + x_difference)
+ front_2.append(front_middle[1] + y_difference)
+
+ back_1 = []
+ back_1.append(back_middle[0] - x_difference)
+ back_1.append(back_middle[1] - y_difference)
+
+ back_2 = []
+ back_2.append(back_middle[0] + x_difference)
+ back_2.append(back_middle[1] + y_difference)
+
+ x_difference = x_difference_o * mToPx(
+ LENGTH_OF_ROBOT / 2 + ROBOT_SIDE_TO_BALL_CENTER) / distance
+ y_difference = y_difference_o * mToPx(
+ LENGTH_OF_ROBOT / 2 + ROBOT_SIDE_TO_BALL_CENTER) / distance
+
+ #Calculate Ball
+ ball_center = []
+ ball_center.append(p1[0] + x_difference)
+ ball_center.append(p1[1] + y_difference)
+
+ x_difference = x_difference_o * mToPx(
+ LENGTH_OF_ROBOT / 2 + ROBOT_SIDE_TO_HATCH_PANEL) / distance
+ y_difference = y_difference_o * mToPx(
+ LENGTH_OF_ROBOT / 2 + ROBOT_SIDE_TO_HATCH_PANEL) / distance
+
+ #Calculate Panel
+ panel_center = []
+ panel_center.append(p1[0] + x_difference)
+ panel_center.append(p1[1] + y_difference)
+
+ x_difference = np.sin(angle[0]) * mToPx(HATCH_PANEL_WIDTH / 2)
+ y_difference = np.cos(angle[0]) * mToPx(HATCH_PANEL_WIDTH / 2)
+
+ panel_1 = []
+ panel_1.append(panel_center[0] + x_difference)
+ panel_1.append(panel_center[1] + y_difference)
+
+ panel_2 = []
+ panel_2.append(panel_center[0] - x_difference)
+ panel_2.append(panel_center[1] - y_difference)
+
+ #Draw Robot
+ cr.move_to(front_1[0], front_1[1])
+ cr.line_to(back_1[0], back_1[1])
+ cr.line_to(back_2[0], back_2[1])
+ cr.line_to(front_2[0], front_2[1])
+ cr.line_to(front_1[0], front_1[1])
+
+ cr.stroke()
+
+ #Draw Ball
+ set_color(cr, palette["ORANGE"], 0.5)
+ cr.move_to(back_middle[0], back_middle[1])
+ cr.line_to(ball_center[0], ball_center[1])
+ cr.arc(ball_center[0], ball_center[1], mToPx(BALL_RADIUS), 0,
+ 2 * np.pi)
+ cr.stroke()
+
+ #Draw Panel
+ set_color(cr, palette["YELLOW"], 0.5)
+ cr.move_to(panel_1[0], panel_1[1])
+ cr.line_to(panel_2[0], panel_2[1])
+
+ cr.stroke()
+ cr.set_source_rgba(0, 0, 0, 1)
+
+ def handle_draw(self, cr): # main
# Fill the background color of the window with grey
- set_color(cr, palette["GREY"])
+ set_color(cr, palette["WHITE"])
cr.paint()
- #Scale the field to fit within drawing area
- cr.scale(0.5, 0.5)
# Draw a extents rectangle
set_color(cr, palette["WHITE"])
@@ -378,209 +206,181 @@
self.extents_y_max - self.extents_y_min)
cr.fill()
- #Drawing the field
+ #Drawing the switch and scale in the field
cr.move_to(0, 50)
cr.show_text('Press "e" to export')
cr.show_text('Press "i" to import')
- set_color(cr, Color(0.3, 0.3, 0.3))
- cr.rectangle(-450, -150, 300, 300)
+ set_color(cr, palette["WHITE"])
+ cr.rectangle(0, -mToPx(8.2296 / 2.0), SCREEN_SIZE, SCREEN_SIZE)
cr.fill()
set_color(cr, palette["BLACK"])
- cr.rectangle(-450, -150, 300, 300)
+ cr.rectangle(0, -mToPx(8.2296 / 2.0), SCREEN_SIZE, SCREEN_SIZE)
cr.set_line_join(cairo.LINE_JOIN_ROUND)
cr.stroke()
-
self.draw_field_elements(cr)
-
y = 0
- # update all the things
+ # update everything
- if self.mode == Mode.kViewing:
+ if self.mode == Mode.kPlacing or self.mode == Mode.kViewing:
set_color(cr, palette["BLACK"])
- cr.move_to(-300, 170)
- cr.show_text("VIEWING")
- set_color(cr, palette["GREY"])
-
- if len(self.selected_points) > 0:
- print("SELECTED_POINTS: " + str(len(self.selected_points)))
- print("ITEMS:")
- # for item in self.selected_points:
- # print(str(item))
- for i, point in enumerate(self.selected_points):
- # print("I: " + str(i))
- draw_px_x(cr, point[0], point[1], 10)
- cr.move_to(point[0], point[1] - 15)
- display_text(cr, str(i), 0.5, 0.5, 2, 2)
-
- elif self.mode == Mode.kPlacing:
- set_color(cr, palette["BLACK"])
- cr.move_to(-300, 170)
- display_text(cr, "ADD", 1, 1, 1, 1)
- set_color(cr, palette["GREY"])
-
- if len(self.selected_points) > 0:
- print("SELECTED_POINTS: " + str(len(self.selected_points)))
- print("ITEMS:")
- for item in self.selected_points:
- print(str(item))
- for i, point in enumerate(self.selected_points):
- print("I: " + str(i))
+ cr.move_to(-SCREEN_SIZE, 170)
+ plotPoints = self.points.getPoints()
+ if plotPoints:
+ for i, point in enumerate(plotPoints):
draw_px_x(cr, mToPx(point[0]), mToPx(point[1]), 10)
cr.move_to(mToPx(point[0]), mToPx(point[1]) - 15)
display_text(cr, str(i), 0.5, 0.5, 2, 2)
+ set_color(cr, palette["WHITE"])
elif self.mode == Mode.kEditing:
set_color(cr, palette["BLACK"])
- cr.move_to(-300, 170)
+ cr.move_to(-SCREEN_SIZE, 170)
display_text(cr, "EDITING", 1, 1, 1, 1)
- if len(self.splines) > 0:
- holder_spline = []
- for i, points in enumerate(self.splines):
- array = np.zeros(shape=(6, 2), dtype=float)
- for j, point in enumerate(points):
- array[j, 0] = mToPx(point[0])
- array[j, 1] = mToPx(point[1])
- spline = Spline(np.ascontiguousarray(np.transpose(array)))
- for k in np.linspace(0.01, 1, 100):
- cr.move_to(
- spline.Point(k - 0.01)[0],
- spline.Point(k - 0.01)[1])
- cr.line_to(spline.Point(k)[0], spline.Point(k)[1])
- cr.stroke()
- holding = [
- spline.Point(k - 0.01)[0],
- spline.Point(k - 0.01)[1]
- ]
- holder_spline.append(holding)
- self.curves.append(holder_spline)
+ if self.points.getSplines():
+ self.draw_splines(cr)
+ for i, points in enumerate(self.points.getSplines()):
- for spline, points in enumerate(self.splines):
- # for item in points:
- # print(str(item))
- for i, point in enumerate(points):
- # print("I: " + str(i))
- if spline == self.spline_edit and i == self.index_of_edit:
- draw_px_x(cr, mToPx(point[0]), mToPx(point[1]), 15,
- self.colors[spline])
- elif (spline == 0 and not i == 5) or (not i == 0
- and not i == 5):
- draw_px_x(cr, mToPx(point[0]), mToPx(point[1]), 10,
- self.colors[spline])
- cr.move_to(mToPx(point[0]), mToPx(point[1]) - 15)
- display_text(cr, str(i), 0.5, 0.5, 2, 2)
+ p0 = np.array([mToPx(points[0][0]), mToPx(points[0][1])])
+ p1 = np.array([mToPx(points[1][0]), mToPx(points[1][1])])
+ p2 = np.array([mToPx(points[2][0]), mToPx(points[2][1])])
+ p3 = np.array([mToPx(points[3][0]), mToPx(points[3][1])])
+ p4 = np.array([mToPx(points[4][0]), mToPx(points[4][1])])
+ p5 = np.array([mToPx(points[5][0]), mToPx(points[5][1])])
- elif self.mode == Mode.kConstraint:
- print("Drawn")
- set_color(cr, palette["BLACK"])
- cr.move_to(-300, 170)
- display_text(cr, "Adding Constraint", 1, 1, 1, 1)
- if len(self.splines) > 0:
- # print("Splines: " + str(len(self.splines)))
- # print("ITEMS:")
- for s, points in enumerate(self.splines):
- # for item in points:
- # print(str(item))
- for i, point in enumerate(points):
- # print("I: " + str(i))
- draw_px_x(cr, point[0], point[1], 10, self.colors[s])
- cr.move_to(point[0], point[1] - 15)
- display_text(cr, str(i), 0.5, 0.5, 2, 2)
+ draw_control_points(cr, [p0, p1, p2, p3, p4, p5])
+ first_tangent = p0 + 2.0 * (p1 - p0)
+ second_tangent = p5 + 2.0 * (p4 - p5)
+ cr.set_source_rgb(0, 0.5, 0)
+ cr.move_to(p0[0], p0[1])
+ cr.set_line_width(1.0)
+ cr.line_to(first_tangent[0], first_tangent[1])
+ cr.move_to(first_tangent[0], first_tangent[1])
+ cr.line_to(p2[0], p2[1])
- cr.paint_with_alpha(.65)
+ cr.move_to(p5[0], p5[1])
+ cr.line_to(second_tangent[0], second_tangent[1])
+ cr.move_to(second_tangent[0], second_tangent[1])
+ cr.line_to(p3[0], p3[1])
+
+ cr.stroke()
+ cr.set_line_width(2.0)
+ self.points.update_lib_spline()
+ set_color(cr, palette["WHITE"])
+
+ cr.paint_with_alpha(0.2)
+
+ mygraph = Graph(cr, self.points)
draw_px_cross(cr, self.x, self.y, 10)
- def do_key_press(self, event):
+ def draw_splines(self, cr):
+ holder_spline = []
+ for i, points in enumerate(self.points.getSplines()):
+ array = np.zeros(shape=(6, 2), dtype=float)
+ for j, point in enumerate(points):
+ array[j, 0] = point[0]
+ array[j, 1] = point[1]
+ spline = Spline(np.ascontiguousarray(np.transpose(array)))
+ for k in np.linspace(0.01, 1, 100):
+ cr.move_to(mToPx(spline.Point(k - 0.01)[0]),
+ mToPx(spline.Point(k - 0.01)[1]))
+ cr.line_to(mToPx(spline.Point(k)[0]),
+ mToPx(spline.Point(k)[1]))
+ cr.stroke()
+ holding = [
+ spline.Point(k - 0.01)[0],
+ spline.Point(k - 0.01)[1]
+ ]
+ holder_spline.append(holding)
+ if i == 0:
+ self.draw_robot_at_point(cr, 0.00, 0.01, spline)
+ self.draw_robot_at_point(cr, 1, 0.01, spline)
+ self.curves.append(holder_spline)
+
+ def mouse_move(self, event):
+ old_x = self.x
+ old_y = self.y
+ self.x = event.x
+ self.y = event.y
+ dif_x = event.x - old_x
+ dif_y = event.y - old_y
+ difs = np.array([pxToM(dif_x), pxToM(dif_y)])
+
+ if self.mode == Mode.kEditing:
+ self.spline_edit = self.points.updates_for_mouse_move(
+ self.index_of_edit, self.spline_edit, self.x, self.y, difs)
+
+ def do_key_press(self, event, file_name):
keyval = Gdk.keyval_to_lower(event.keyval)
- # print("Gdk.KEY_" + Gdk.keyval_name(keyval))
+ module_path = os.path.dirname(os.path.realpath(sys.argv[0]))
+ self.path_to_export = os.path.join(module_path,
+ "spline_jsons/" + file_name)
if keyval == Gdk.KEY_q:
print("Found q key and exiting.")
quit_main_loop()
- if keyval == Gdk.KEY_e:
- # Will export to json file
- self.mode = Mode.kEditing
- print(str(sys.argv))
- print('out to: ', self.path_to_export)
- exportList = [l.tolist() for l in self.splines]
- with open(self.path_to_export, mode='w') as points_file:
- json.dump(exportList, points_file)
- print("Wrote: " + str(self.splines))
- if keyval == Gdk.KEY_i:
- # import from json file
- self.mode = Mode.kEditing
- self.selected_points = []
- self.splines = []
- with open(self.path_to_export) as points_file:
- self.splines = json.load(points_file)
- print("Added: " + str(self.splines))
+ file_name_end = file_name[-5:]
+ if file_name_end != ".json":
+ print("Error: Filename doesn't end in .json")
+ else:
+ if keyval == Gdk.KEY_e:
+ # Will export to json file
+ self.mode = Mode.kEditing
+ print('out to: ', self.path_to_export)
+ exportList = [l.tolist() for l in self.points.getSplines()]
+ with open(self.path_to_export, mode='w') as points_file:
+ json.dump(exportList, points_file)
+
+ if keyval == Gdk.KEY_i:
+ # import from json file
+ self.mode = Mode.kEditing
+ self.points.resetPoints()
+ self.points.resetSplines()
+ with open(self.path_to_export) as points_file:
+ self.points.setUpSplines(json.load(points_file))
+
+ self.points.update_lib_spline()
+
if keyval == Gdk.KEY_p:
self.mode = Mode.kPlacing
# F0 = A1
# B1 = 2F0 - E0
# C1= d0 + 4F0 - 4E0
- spline_index = len(self.splines) - 1
- self.selected_points = []
- f = self.splines[spline_index][5]
- e = self.splines[spline_index][4]
- d = self.splines[spline_index][3]
- self.selected_points.append(f)
- self.selected_points.append(f * 2 + e * -1)
- self.selected_points.append(d + f * 4 + e * -4)
-
- if keyval == Gdk.KEY_c:
- self.mode = Mode.kConstraint
+ spline_index = len(self.points.getSplines()) - 1
+ self.points.resetPoints()
+ self.points.extrapolate(
+ self.points.getSplines()[len(self.points.getSplines()) - 1][5],
+ self.points.getSplines()[len(self.points.getSplines()) - 1][4],
+ self.points.getSplines()[len(self.points.getSplines()) - 1][3])
def button_press_action(self):
if self.mode == Mode.kPlacing:
- #Check that the point clicked is on the field
- if (self.x < -150 and self.x > -450 and self.y < 150
- and self.y > -150):
- self.add_point(self.x, self.y)
+ if self.points.add_point(self.x, self.y):
+ self.mode = Mode.kEditing
elif self.mode == Mode.kEditing:
# Now after index_of_edit is not -1, the point is selected, so
# user can click for new point
if self.index_of_edit > -1 and self.held_x != self.x:
- print("INDEX OF EDIT: " + str(self.index_of_edit))
- self.splines[self.spline_edit][self.index_of_edit] = [
- pxToM(self.x), pxToM(self.y)
- ]
+ self.points.setSplines(self.spline_edit, self.index_of_edit,
+ pxToM(self.x), pxToM(self.y))
- if not self.spline_edit == len(self.splines) - 1:
- spline_edit = self.spline_edit + 1
- f = self.splines[self.spline_edit][5]
- e = self.splines[self.spline_edit][4]
- d = self.splines[self.spline_edit][3]
- self.splines[spline_edit][0] = f
- self.splines[spline_edit][1] = f * 2 + e * -1
- self.splines[spline_edit][2] = d + f * 4 + e * -4
-
- if not self.spline_edit == 0:
- spline_edit = self.spline_edit - 1
- a = self.splines[self.spline_edit][0]
- b = self.splines[self.spline_edit][1]
- c = self.splines[self.spline_edit][2]
- self.splines[spline_edit][5] = a
- self.splines[spline_edit][4] = a * 2 + b * -1
- self.splines[spline_edit][3] = c + a * 4 + b * -4
+ self.spline_edit = self.points.splineExtrapolate(
+ self.spline_edit)
self.index_of_edit = -1
self.spline_edit = -1
else:
- print("mode == 2")
# Get clicked point
# Find nearest
# Move nearest to clicked
cur_p = [pxToM(self.x), pxToM(self.y)]
- print("CUR_P: " + str(self.x) + " " + str(self.y))
# Get the distance between each for x and y
# Save the index of the point closest
nearest = 1 # Max distance away a the selected point can be in meters
index_of_closest = 0
- for index_splines, points in enumerate(self.splines):
+ for index_splines, points in enumerate(self.points.getSplines()):
for index_points, val in enumerate(points):
- # pythagorean
distance = np.sqrt((cur_p[0] - val[0])**2 +
(cur_p[1] - val[1])**2)
if distance < nearest:
@@ -591,10 +391,6 @@
self.index_of_edit = index_of_closest
self.spline_edit = index_splines
self.held_x = self.x
- elif self.mode == Mode.kConstraint:
- print("RAN")
- self.set_index_to_nearest_spline_point()
- print("FINISHED")
def do_button_press(self, event):
print("button press activated")
@@ -612,13 +408,23 @@
print("Method_connect ran")
self.connect(event, handler)
+ def mouse_move(self, event):
+ #Changes event.x and event.y to be relative to the center.
+ x = event.x - self.drawing_area.window_shape[0] / 2
+ y = self.drawing_area.window_shape[1] / 2 - event.y
+ scale = self.drawing_area.get_current_scale()
+ event.x = x / scale + self.drawing_area.center[0]
+ event.y = y / scale + self.drawing_area.center[1]
+ self.drawing_area.mouse_move(event)
+ self.queue_draw()
+
def button_press(self, event):
print("button press activated")
o_x = event.x
o_y = event.y
x = event.x - self.drawing_area.window_shape[0] / 2
y = self.drawing_area.window_shape[1] / 2 - event.y
- scale = self.drawing_area.get_current_scale()
+ scale = 2 * self.drawing_area.get_current_scale()
event.x = x / scale + self.drawing_area.center[0]
event.y = y / scale + self.drawing_area.center[1]
self.drawing_area.do_button_press(event)
@@ -627,21 +433,17 @@
def key_press(self, event):
print("key press activated")
- self.drawing_area.do_key_press(event)
+ self.drawing_area.do_key_press(event, self.file_name_box.get_text())
self.queue_draw()
def configure(self, event):
print("configure activated")
self.drawing_area.window_shape = (event.width, event.height)
- def on_submit_click(self, widget):
- self.drawing_area.inConstraint = int(self.constraint_box.get_text())
- self.drawing_area.inValue = int(self.value_box.get_text())
-
def __init__(self):
Gtk.Window.__init__(self)
- self.set_default_size(1366, 738)
+ self.set_default_size(1.5 * SCREEN_SIZE, SCREEN_SIZE)
flowBox = Gtk.FlowBox()
flowBox.set_valign(Gtk.Align.START)
@@ -663,41 +465,25 @@
| Gdk.EventMask.SCROLL_MASK
| Gdk.EventMask.KEY_PRESS_MASK)
+ #add the graph box
self.drawing_area = GTK_Widget()
self.eventBox.add(self.drawing_area)
self.method_connect("key-release-event", self.key_press)
self.method_connect("button-release-event", self.button_press)
self.method_connect("configure-event", self.configure)
+ self.method_connect("motion_notify_event", self.mouse_move)
- # Constraint Boxes
+ self.file_name_box = Gtk.Entry()
+ self.file_name_box.set_size_request(200, 40)
- self.start_box = Gtk.Entry()
- self.start_box.set_size_request(100, 20)
+ self.file_name_box.set_text("File")
+ self.file_name_box.set_editable(True)
- self.constraint_box = Gtk.Entry()
- self.constraint_box.set_size_request(100, 20)
-
- self.constraint_box.set_text("Constraint")
- self.constraint_box.set_editable(True)
-
- container.put(self.constraint_box, 700, 0)
-
- self.value_box = Gtk.Entry()
- self.value_box.set_size_request(100, 20)
-
- self.value_box.set_text("Value")
- self.value_box.set_editable(True)
-
- container.put(self.value_box, 700, 40)
-
- self.submit_button = Gtk.Button("Submit")
- self.submit_button.connect('clicked', self.on_submit_click)
-
- container.put(self.submit_button, 880, 0)
+ container.put(self.file_name_box, 0, 0)
self.show_all()
window = GridWindow()
-basic_window.RunApp()
+RunApp()
diff --git a/frc971/control_loops/python/points.py b/frc971/control_loops/python/points.py
new file mode 100644
index 0000000..8f94e3f
--- /dev/null
+++ b/frc971/control_loops/python/points.py
@@ -0,0 +1,110 @@
+from constants import *
+import numpy as np
+from libspline import Spline, DistanceSpline, Trajectory
+
+
+class Points():
+ def __init__(self):
+ self.points = [] # Holds all points not yet in spline
+ self.libsplines = [] # Formatted for libspline library usage
+ self.splines = [] # Formatted for drawing
+
+ def getPoints(self):
+ return self.points
+
+ def resetPoints(self):
+ self.points = []
+
+ def getLibsplines(self):
+ return self.libsplines
+
+ def splineExtrapolate(self, o_spline_edit):
+ spline_edit = o_spline_edit
+ if not spline_edit == len(self.splines) - 1:
+ spline_edit = spline_edit + 1
+ f = self.splines[spline_edit][5]
+ e = self.splines[spline_edit][4]
+ d = self.splines[spline_edit][3]
+ self.splines[spline_edit][0] = f
+ self.splines[spline_edit][1] = f * 2 + e * -1
+ self.splines[spline_edit][2] = d + f * 4 + e * -4
+
+ if not spline_edit == 0:
+ spline_edit = spline_edit - 1
+ a = self.splines[spline_edit][0]
+ b = self.splines[spline_edit][1]
+ c = self.splines[spline_edit][2]
+ self.splines[spline_edit][5] = a
+ self.splines[spline_edit][4] = a * 2 + b * -1
+ self.splines[spline_edit][3] = c + a * 4 + b * -4
+
+ return spline_edit
+
+ def updates_for_mouse_move(self, index_of_edit, spline_edit, x, y, difs):
+ if index_of_edit > -1:
+ self.splines[spline_edit][index_of_edit] = [pxToM(x), pxToM(y)]
+
+ if index_of_edit == 5:
+ self.splines[spline_edit][
+ index_of_edit -
+ 2] = self.splines[spline_edit][index_of_edit - 2] + difs
+ self.splines[spline_edit][
+ index_of_edit -
+ 1] = self.splines[spline_edit][index_of_edit - 1] + difs
+
+ if index_of_edit == 0:
+ self.splines[spline_edit][
+ index_of_edit +
+ 2] = self.splines[spline_edit][index_of_edit + 2] + difs
+ self.splines[spline_edit][
+ index_of_edit +
+ 1] = self.splines[spline_edit][index_of_edit + 1] + difs
+
+ if index_of_edit == 4:
+ self.splines[spline_edit][
+ index_of_edit -
+ 1] = self.splines[spline_edit][index_of_edit - 1] + difs
+
+ if index_of_edit == 1:
+ self.splines[spline_edit][
+ index_of_edit +
+ 1] = self.splines[spline_edit][index_of_edit + 1] + difs
+
+ return self.splineExtrapolate(spline_edit)
+
+ def update_lib_spline(self):
+ self.libsplines = []
+ array = np.zeros(shape=(6, 2), dtype=float)
+ for points in self.splines:
+ for j, point in enumerate(points):
+ array[j, 0] = point[0]
+ array[j, 1] = point[1]
+ spline = Spline(np.ascontiguousarray(np.transpose(array)))
+ self.libsplines.append(spline)
+
+ def getSplines(self):
+ return self.splines
+
+ def resetSplines(self):
+ self.splines = []
+
+ def setUpSplines(self, newSplines):
+ self.splines = newSplines
+
+ def setSplines(self, spline_edit, index_of_edit, x, y):
+ self.splines[spline_edit][index_of_edit] = [x, y]
+
+ def add_point(self, x, y):
+ if (len(self.points) < 6):
+ self.points.append([pxToM(x), pxToM(y)])
+ if (len(self.points) == 6):
+ self.splines.append(np.array(self.points))
+ self.points = []
+ self.update_lib_spline()
+ return True
+
+ def extrapolate(self, point1, point2,
+ point3): # where point3 is 3rd to last point
+ self.points.append(point1)
+ self.points.append(point1 * 2 - point2)
+ self.points.append(point3 + point1 * 4 - point2 * 4)
diff --git a/frc971/control_loops/python/spline.py b/frc971/control_loops/python/spline.py
index e69d874..b428c9a 100644
--- a/frc971/control_loops/python/spline.py
+++ b/frc971/control_loops/python/spline.py
@@ -15,7 +15,6 @@
from frc971.control_loops.python import drivetrain
from frc971.control_loops.python import controls
import y2016.control_loops.python.drivetrain
-
"""This file is my playground for implementing spline following.
All splines here are cubic bezier splines. See
@@ -73,8 +72,9 @@
if numpy.isscalar(alpha):
alpha = [alpha]
dalpha_matrix = [[
- -3.0 * (1.0 - a)**2.0, 3.0 * (1.0 - a)**2.0 + -2.0 * 3.0 *
- (1.0 - a) * a, -3.0 * a**2.0 + 2.0 * 3.0 * (1.0 - a) * a, 3.0 * a**2.0
+ -3.0 * (1.0 - a)**2.0,
+ 3.0 * (1.0 - a)**2.0 + -2.0 * 3.0 * (1.0 - a) * a,
+ -3.0 * a**2.0 + 2.0 * 3.0 * (1.0 - a) * a, 3.0 * a**2.0
] for a in alpha]
return control_points * numpy.matrix(dalpha_matrix).T
@@ -97,8 +97,7 @@
ddalpha_matrix = [[
2.0 * 3.0 * (1.0 - a),
-2.0 * 3.0 * (1.0 - a) + -2.0 * 3.0 * (1.0 - a) + 2.0 * 3.0 * a,
- -2.0 * 3.0 * a + 2.0 * 3.0 * (1.0 - a) - 2.0 * 3.0 * a,
- 2.0 * 3.0 * a
+ -2.0 * 3.0 * a + 2.0 * 3.0 * (1.0 - a) - 2.0 * 3.0 * a, 2.0 * 3.0 * a
] for a in alpha]
return control_points * numpy.matrix(ddalpha_matrix).T
@@ -119,10 +118,8 @@
if numpy.isscalar(alpha):
alpha = [alpha]
ddalpha_matrix = [[
- -2.0 * 3.0,
- 2.0 * 3.0 + 2.0 * 3.0 + 2.0 * 3.0,
- -2.0 * 3.0 - 2.0 * 3.0 - 2.0 * 3.0,
- 2.0 * 3.0
+ -2.0 * 3.0, 2.0 * 3.0 + 2.0 * 3.0 + 2.0 * 3.0,
+ -2.0 * 3.0 - 2.0 * 3.0 - 2.0 * 3.0, 2.0 * 3.0
] for a in alpha]
return control_points * numpy.matrix(ddalpha_matrix).T
@@ -143,7 +140,8 @@
dspline_points = dspline(alpha, control_points)
return numpy.arctan2(
- numpy.array(dspline_points)[1, :], numpy.array(dspline_points)[0, :])
+ numpy.array(dspline_points)[1, :],
+ numpy.array(dspline_points)[0, :])
def dspline_theta(alpha,
@@ -219,8 +217,8 @@
dddy = numpy.array(dddspline_points)[1, :]
return -1.0 / ((dx**2.0 + dy**2.0)**2.0) * (dx * ddy - dy * ddx) * 2.0 * (
- dy * ddy + dx * ddx) + 1.0 / (dx**2.0 + dy**2.0) * (dx * dddy - dy *
- dddx)
+ dy * ddy + dx * ddx) + 1.0 / (dx**2.0 + dy**2.0) * (dx * dddy -
+ dy * dddx)
class Path(object):
@@ -230,7 +228,8 @@
self._control_points = control_points
def spline_velocity(alpha):
- return numpy.linalg.norm(dspline(alpha, self._control_points), axis=0)
+ return numpy.linalg.norm(dspline(alpha, self._control_points),
+ axis=0)
self._point_distances = [0.0]
num_alpha = 100
@@ -240,8 +239,8 @@
# might think.
for alpha in numpy.linspace(0.0, 1.0, num_alpha)[1:]:
self._point_distances.append(
- scipy.integrate.fixed_quad(spline_velocity, alpha, alpha + 1.0
- / (num_alpha - 1.0))[0] +
+ scipy.integrate.fixed_quad(spline_velocity, alpha, alpha +
+ 1.0 / (num_alpha - 1.0))[0] +
self._point_distances[-1])
def distance_to_alpha(self, distance):
@@ -256,7 +255,8 @@
if numpy.isscalar(distance):
return numpy.array([self._distance_to_alpha_scalar(distance)])
else:
- return numpy.array([self._distance_to_alpha_scalar(d) for d in distance])
+ return numpy.array(
+ [self._distance_to_alpha_scalar(d) for d in distance])
def _distance_to_alpha_scalar(self, distance):
"""Helper to compute alpha for a distance for a single scalar."""
@@ -264,15 +264,17 @@
return 0.0
elif distance >= self.length():
return 1.0
- after_index = numpy.searchsorted(
- self._point_distances, distance, side='right')
+ after_index = numpy.searchsorted(self._point_distances,
+ distance,
+ side='right')
before_index = after_index - 1
# Linearly interpolate alpha from our (sorted) distance table.
return (distance - self._point_distances[before_index]) / (
self._point_distances[after_index] -
- self._point_distances[before_index]) * (1.0 / (
- len(self._point_distances) - 1.0)) + float(before_index) / (
+ self._point_distances[before_index]) * (
+ 1.0 /
+ (len(self._point_distances) - 1.0)) + float(before_index) / (
len(self._point_distances) - 1.0)
def length(self):
@@ -287,8 +289,8 @@
# TODO(austin): need a better name...
def dxy(self, distance):
"""Returns the xy velocity as a function of distance."""
- dspline_point = dspline(
- self.distance_to_alpha(distance), self._control_points)
+ dspline_point = dspline(self.distance_to_alpha(distance),
+ self._control_points)
return dspline_point / numpy.linalg.norm(dspline_point, axis=0)
# TODO(austin): need a better name...
@@ -298,8 +300,7 @@
dspline_points = dspline(alpha, self._control_points)
ddspline_points = ddspline(alpha, self._control_points)
- norm = numpy.linalg.norm(
- dspline_points, axis=0)**2.0
+ norm = numpy.linalg.norm(dspline_points, axis=0)**2.0
return ddspline_points / norm - numpy.multiply(
dspline_points, (numpy.array(dspline_points)[0, :] *
@@ -309,10 +310,9 @@
def theta(self, distance, dspline_points=None):
"""Returns the heading as a function of distance."""
- return spline_theta(
- self.distance_to_alpha(distance),
- self._control_points,
- dspline_points=dspline_points)
+ return spline_theta(self.distance_to_alpha(distance),
+ self._control_points,
+ dspline_points=dspline_points)
def dtheta(self, distance, dspline_points=None, ddspline_points=None):
"""Returns the angular velocity as a function of distance."""
@@ -327,9 +327,14 @@
return dtheta_points / numpy.linalg.norm(dspline_points, axis=0)
- def dtheta_dt(self, distance, velocity, dspline_points=None, ddspline_points=None):
+ def dtheta_dt(self,
+ distance,
+ velocity,
+ dspline_points=None,
+ ddspline_points=None):
"""Returns the angular velocity as a function of time."""
- return self.dtheta(distance, dspline_points, ddspline_points) * velocity
+ return self.dtheta(distance, dspline_points,
+ ddspline_points) * velocity
def ddtheta(self,
distance,
@@ -400,8 +405,8 @@
return 0.0
return (f(t, y) - a0) / y
- return (RungeKutta(integrablef, v, x, dx) - v
- ) + numpy.sqrt(2.0 * a0 * dx + v * v)
+ return (RungeKutta(integrablef, v, x, dx) - v) + numpy.sqrt(2.0 * a0 * dx +
+ v * v)
class Trajectory(object):
@@ -409,8 +414,8 @@
distance_count):
self._path = path
self._drivetrain = drivetrain
- self.distances = numpy.linspace(0.0,
- self._path.length(), distance_count)
+ self.distances = numpy.linspace(0.0, self._path.length(),
+ distance_count)
self._longitudal_accel = longitudal_accel
self._lateral_accel = lateral_accel
@@ -460,9 +465,10 @@
C = K3 * v * v + K4 * v
# Note: K345 are not quite constant over the step, but we are going
# to assume they are for now.
- accelerations = [(12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) /
- K5[1, 0], (-12.0 - C[0, 0]) / K5[0, 0],
- (-12.0 - C[1, 0]) / K5[1, 0]]
+ accelerations = [
+ (12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) / K5[1, 0],
+ (-12.0 - C[0, 0]) / K5[0, 0], (-12.0 - C[1, 0]) / K5[1, 0]
+ ]
maxa = -float('inf')
for a in accelerations:
U = K5 * a + K3 * v * v + K4 * v
@@ -473,8 +479,8 @@
# Constrain the longitudinal acceleration to keep in a pseudo friction
# circle. This will make it so we don't floor it while in a turn and
# cause extra wheel slip.
- long_accel = numpy.sqrt(1.0 - (lateral_accel / self._lateral_accel)**
- 2.0) * self._longitudal_accel
+ long_accel = numpy.sqrt(1.0 - (
+ lateral_accel / self._lateral_accel)**2.0) * self._longitudal_accel
return min(long_accel, maxa)
def forward_pass(self, plan):
@@ -503,9 +509,10 @@
C = K3 * v * v + K4 * v
# Note: K345 are not quite constant over the step, but we are going
# to assume they are for now.
- accelerations = [(12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) /
- K5[1, 0], (-12.0 - C[0, 0]) / K5[0, 0],
- (-12.0 - C[1, 0]) / K5[1, 0]]
+ accelerations = [
+ (12.0 - C[0, 0]) / K5[0, 0], (12.0 - C[1, 0]) / K5[1, 0],
+ (-12.0 - C[0, 0]) / K5[0, 0], (-12.0 - C[1, 0]) / K5[1, 0]
+ ]
mina = float('inf')
for a in accelerations:
U = K5 * a + K3 * v * v + K4 * v
@@ -516,8 +523,8 @@
# Constrain the longitudinal acceleration to keep in a pseudo friction
# circle. This will make it so we don't floor it while in a turn and
# cause extra wheel slip.
- long_accel = -numpy.sqrt(1.0 - (lateral_accel / self._lateral_accel)**
- 2.0) * self._longitudal_accel
+ long_accel = -numpy.sqrt(1.0 - (
+ lateral_accel / self._lateral_accel)**2.0) * self._longitudal_accel
return max(long_accel, mina)
def backward_pass(self, plan):
@@ -546,8 +553,9 @@
if distance > self.distances[-1]:
distance = self.distances[-1]
else:
- after_index = numpy.searchsorted(
- self.distances, distance, side='right')
+ after_index = numpy.searchsorted(self.distances,
+ distance,
+ side='right')
before_index = after_index - 1
vforwards = integrate_accel_for_distance(
@@ -586,14 +594,15 @@
return U
def goal_state(self, distance, velocity):
- width = (
- self._drivetrain.robot_radius_l + self._drivetrain.robot_radius_r)
+ width = (self._drivetrain.robot_radius_l +
+ self._drivetrain.robot_radius_r)
goal = numpy.matrix(numpy.zeros((5, 1)))
goal[0:2, :] = self._path.xy(distance)
goal[2, :] = self._path.theta(distance)
- Ter = numpy.linalg.inv(numpy.matrix([[0.5, 0.5], [-1.0 / width, 1.0 / width]]))
+ Ter = numpy.linalg.inv(
+ numpy.matrix([[0.5, 0.5], [-1.0 / width, 1.0 / width]]))
goal[3:5, :] = Ter * numpy.matrix(
[[velocity], [self._path.dtheta_dt(distance, velocity)]])
return goal
@@ -618,25 +627,23 @@
# Compute theta and the two derivatives
theta = spline_theta(alphas, control_points, dspline_points=dspline_points)
- dtheta = dspline_theta(
- alphas, control_points, dspline_points=dspline_points)
- ddtheta = ddspline_theta(
- alphas,
- control_points,
- dspline_points=dspline_points,
- dddspline_points=dddspline_points)
+ dtheta = dspline_theta(alphas,
+ control_points,
+ dspline_points=dspline_points)
+ ddtheta = ddspline_theta(alphas,
+ control_points,
+ dspline_points=dspline_points,
+ dddspline_points=dddspline_points)
# Plot the control points and the spline.
pylab.figure()
- pylab.plot(
- numpy.array(control_points)[0, :],
- numpy.array(control_points)[1, :],
- '-o',
- label='control')
- pylab.plot(
- numpy.array(spline_points)[0, :],
- numpy.array(spline_points)[1, :],
- label='spline')
+ pylab.plot(numpy.array(control_points)[0, :],
+ numpy.array(control_points)[1, :],
+ '-o',
+ label='control')
+ pylab.plot(numpy.array(spline_points)[0, :],
+ numpy.array(spline_points)[1, :],
+ label='spline')
pylab.legend()
# For grins, confirm that the double integral of the acceleration (with
@@ -657,9 +664,9 @@
# Integrate up the spline velocity and heading to confirm that given a
# velocity (as a function of the spline parameter) and angle, we will move
# from the starting point to the ending point.
- thetaint_plot = numpy.zeros((len(alphas),))
+ thetaint_plot = numpy.zeros((len(alphas), ))
thetaint = theta[0]
- dthetaint_plot = numpy.zeros((len(alphas),))
+ dthetaint_plot = numpy.zeros((len(alphas), ))
dthetaint = dtheta[0]
thetaint_plot[0] = thetaint
dthetaint_plot[0] = dthetaint
@@ -670,15 +677,14 @@
for i in range(len(alphas) - 1):
dalpha = alphas[i + 1] - alphas[i]
txint += dalpha * numpy.linalg.norm(
- dspline_points[:, i]) * numpy.matrix(
- [[numpy.cos(theta[i])], [numpy.sin(theta[i])]])
+ dspline_points[:, i]) * numpy.matrix([[numpy.cos(theta[i])],
+ [numpy.sin(theta[i])]])
txint_plot[:, i + 1] = txint
thetaint += dalpha * dtheta[i]
dthetaint += dalpha * ddtheta[i]
thetaint_plot[i + 1] = thetaint
dthetaint_plot[i + 1] = dthetaint
-
# Now plot x, dx/dalpha, ddx/ddalpha, dddx/dddalpha, and integrals thereof
# to perform consistency checks.
pylab.figure()
@@ -709,11 +715,9 @@
pylab.plot(alphas, ddtheta, label='ddtheta')
pylab.plot(alphas, thetaint_plot, label='thetai')
pylab.plot(alphas, dthetaint_plot, label='dthetai')
- pylab.plot(
- alphas,
- numpy.linalg.norm(
- numpy.array(dspline_points), axis=0),
- label='velocity')
+ pylab.plot(alphas,
+ numpy.linalg.norm(numpy.array(dspline_points), axis=0),
+ label='velocity')
# Now, repeat as a function of path length as opposed to alpha
path = Path(control_points)
@@ -780,12 +784,11 @@
longitudal_accel = 3.0
lateral_accel = 2.0
- trajectory = Trajectory(
- path,
- drivetrain=velocity_drivetrain,
- longitudal_accel=longitudal_accel,
- lateral_accel=lateral_accel,
- distance_count=500)
+ trajectory = Trajectory(path,
+ drivetrain=velocity_drivetrain,
+ longitudal_accel=longitudal_accel,
+ lateral_accel=lateral_accel,
+ distance_count=500)
vmax = numpy.inf
vmax = 10.0
@@ -827,14 +830,16 @@
while spline_state[0, 0] < path.length():
t += dt
+
def along_spline_diffeq(t, x):
_, v, a = trajectory.ff_accel(backward_accel_plan, x[0, 0])
return numpy.matrix([[x[1, 0]], [a]])
- spline_state = RungeKutta(along_spline_diffeq,
- spline_state.copy(), t, dt)
+ spline_state = RungeKutta(along_spline_diffeq, spline_state.copy(), t,
+ dt)
- d, v, a = trajectory.ff_accel(backward_accel_plan, length_plan_x[-1][0, 0])
+ d, v, a = trajectory.ff_accel(backward_accel_plan,
+ length_plan_x[-1][0, 0])
length_plan_v.append(v)
length_plan_a.append(a)
@@ -877,9 +882,10 @@
[path.dtheta_dt(xva_plan[0, i], xva_plan[1, i])[0]]])
vel_lr = numpy.linalg.inv(Ter) * vel_omega_r
- state_plan[:, i] = numpy.matrix(
- [[x_r], [y_r], [theta_r], [vel_lr[0, 0]], [vel_lr[1, 0]]])
- u_plan[:, i] = trajectory.ff_voltage(backward_accel_plan, xva_plan[0, i])
+ state_plan[:, i] = numpy.matrix([[x_r], [y_r], [theta_r],
+ [vel_lr[0, 0]], [vel_lr[1, 0]]])
+ u_plan[:, i] = trajectory.ff_voltage(backward_accel_plan, xva_plan[0,
+ i])
Q = numpy.matrix(
numpy.diag([
@@ -906,16 +912,17 @@
linear_velocity = -kMinVelocity
width = (velocity_drivetrain.robot_radius_l +
- velocity_drivetrain.robot_radius_r)
- A_linearized_continuous = numpy.matrix([[
- 0.0, 0.0, -sintheta * linear_velocity, 0.5 * costheta, 0.5 *
- costheta
- ], [
- 0.0, 0.0, costheta * linear_velocity, 0.5 * sintheta, 0.5 *
- sintheta
- ], [0.0, 0.0, 0.0, -1.0 / width, 1.0 / width],
- [0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0]])
+ velocity_drivetrain.robot_radius_r)
+ A_linearized_continuous = numpy.matrix(
+ [[
+ 0.0, 0.0, -sintheta * linear_velocity, 0.5 * costheta,
+ 0.5 * costheta
+ ],
+ [
+ 0.0, 0.0, costheta * linear_velocity, 0.5 * sintheta,
+ 0.5 * sintheta
+ ], [0.0, 0.0, 0.0, -1.0 / width, 1.0 / width],
+ [0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0]])
A_linearized_continuous[3:5, 3:5] = velocity_drivetrain.A_continuous
B_linearized_continuous = numpy.matrix(numpy.zeros((5, 2)))
B_linearized_continuous[3:5, :] = velocity_drivetrain.B_continuous
@@ -942,15 +949,15 @@
velocity_drivetrain.robot_radius_r)
accel = (velocity_drivetrain.A_continuous * velocity +
velocity_drivetrain.B_continuous * U)
- return numpy.matrix(
- [[numpy.cos(theta) * linear_velocity],
- [numpy.sin(theta) * linear_velocity], [angular_velocity],
- [accel[0, 0]], [accel[1, 0]]])
+ return numpy.matrix([[numpy.cos(theta) * linear_velocity],
+ [numpy.sin(theta) * linear_velocity],
+ [angular_velocity], [accel[0, 0]],
+ [accel[1, 0]]])
full_us[:, i] = U
- state = RungeKutta(lambda t, x: spline_diffeq(U, t, x),
- state, i * dt, dt)
+ state = RungeKutta(lambda t, x: spline_diffeq(U, t, x), state, i * dt,
+ dt)
pylab.figure()
pylab.plot(length_plan_t, numpy.array(xva_plan)[0, :], label='x')
@@ -963,17 +970,14 @@
pylab.legend()
pylab.figure()
- pylab.plot(
- numpy.array(states)[0, :],
- numpy.array(states)[1, :],
- label='robot')
- pylab.plot(
- numpy.array(spline_points)[0, :],
- numpy.array(spline_points)[1, :],
- label='spline')
+ pylab.plot(numpy.array(states)[0, :],
+ numpy.array(states)[1, :],
+ label='robot')
+ pylab.plot(numpy.array(spline_points)[0, :],
+ numpy.array(spline_points)[1, :],
+ label='spline')
pylab.legend()
-
def a(_, x):
return 2.0
return 2.0 + 0.0001 * x
diff --git a/frc971/control_loops/python/spline_drawing.py b/frc971/control_loops/python/spline_drawing.py
new file mode 100644
index 0000000..4e8b04b
--- /dev/null
+++ b/frc971/control_loops/python/spline_drawing.py
@@ -0,0 +1,292 @@
+from color import Color, palette
+import cairo
+
+import numpy as np
+from basic_window import OverrideMatrix, identity, quit_main_loop, set_color
+WIDTH_OF_FIELD_IN_METERS = 8.258302
+PIXELS_ON_SCREEN = 800
+
+X_TRANSLATE = 0
+Y_TRANSLATE = 0
+
+
+def pxToM(p):
+ return p * WIDTH_OF_FIELD_IN_METERS / PIXELS_ON_SCREEN
+
+
+def mToPx(m):
+ return (m * PIXELS_ON_SCREEN / WIDTH_OF_FIELD_IN_METERS)
+
+
+def px(cr):
+ return OverrideMatrix(cr, identity)
+
+
+def draw_px_cross(cr, x, y, length_px, color=palette["RED"]):
+ """Draws a cross with fixed dimensions in pixel space."""
+ set_color(cr, color)
+ cr.move_to(x, y - length_px)
+ cr.line_to(x, y + length_px)
+ cr.stroke()
+
+ cr.move_to(x - length_px, y)
+ cr.line_to(x + length_px, y)
+ cr.stroke()
+ set_color(cr, palette["LIGHT_GREY"])
+
+
+def draw_px_x(cr, x, y, length_px1, color=palette["BLACK"]):
+ """Draws a x with fixed dimensions in pixel space."""
+ length_px = length_px1 / np.sqrt(2)
+ set_color(cr, color)
+ cr.move_to(x - length_px, y - length_px)
+ cr.line_to(x + length_px, y + length_px)
+ cr.stroke()
+
+ cr.move_to(x - length_px, y + length_px)
+ cr.line_to(x + length_px, y - length_px)
+ cr.stroke()
+ set_color(cr, palette["LIGHT_GREY"])
+
+
+def display_text(cr, text, widtha, heighta, widthb, heightb):
+ cr.scale(widtha, -heighta)
+ cr.show_text(text)
+ cr.scale(widthb, -heightb)
+
+
+def redraw(needs_redraw, window):
+ print("Redrew")
+ if not needs_redraw:
+ window.queue_draw()
+
+
+def draw_HAB(cr):
+ print("WENT IN")
+ # BASE Constants
+ X_BASE = 0 + X_TRANSLATE #(2.41568)
+ Y_BASE = 0 + Y_TRANSLATE #mToPx(4.129151)
+ BACKWALL_X = X_TRANSLATE
+ LOADING_Y = mToPx(4.129151) - mToPx(0.696976)
+ # HAB Levels 2 and 3 called in variables backhab
+ # draw loading stations
+ cr.move_to(0, LOADING_Y)
+ cr.line_to(mToPx(0.6), LOADING_Y)
+ cr.move_to(0, -1.0 * LOADING_Y)
+ cr.line_to(mToPx(0.6), -1.0 * LOADING_Y)
+
+ BACKWALL_X = X_TRANSLATE
+
+ # HAB Levels 2 and 3 called in variables backhab
+
+ WIDTH_BACKHAB = mToPx(1.2192)
+
+ Y_TOP_BACKHAB_BOX = Y_BASE + mToPx(0.6096)
+ BACKHAB_LV2_LENGTH = mToPx(1.016)
+
+ BACKHAB_LV3_LENGTH = mToPx(1.2192)
+ Y_LV3_BOX = Y_TOP_BACKHAB_BOX - BACKHAB_LV3_LENGTH
+
+ Y_BOTTOM_BACKHAB_BOX = Y_LV3_BOX - BACKHAB_LV2_LENGTH
+
+ # HAB LEVEL 1
+ X_LV1_BOX = BACKWALL_X + WIDTH_BACKHAB
+
+ WIDTH_LV1_BOX = mToPx(0.90805)
+ LENGTH_LV1_BOX = mToPx(1.6256)
+
+ Y_BOTTOM_LV1_BOX = Y_BASE - LENGTH_LV1_BOX
+
+ # Ramp off Level 1
+ X_RAMP = X_LV1_BOX
+
+ Y_TOP_RAMP = Y_BASE + LENGTH_LV1_BOX
+ WIDTH_TOP_RAMP = mToPx(1.20015)
+ LENGTH_TOP_RAMP = Y_BASE + mToPx(0.28306)
+
+ X_MIDDLE_RAMP = X_RAMP + WIDTH_LV1_BOX
+ Y_MIDDLE_RAMP = Y_BOTTOM_LV1_BOX
+ LENGTH_MIDDLE_RAMP = 2 * LENGTH_LV1_BOX
+ WIDTH_MIDDLE_RAMP = WIDTH_TOP_RAMP - WIDTH_LV1_BOX
+
+ Y_BOTTOM_RAMP = Y_BASE - LENGTH_LV1_BOX - LENGTH_TOP_RAMP
+
+ # Side Bars to Hold in balls
+ X_BARS = BACKWALL_X
+ WIDTH_BARS = WIDTH_BACKHAB
+ LENGTH_BARS = mToPx(0.574675)
+
+ Y_TOP_BAR = Y_TOP_BACKHAB_BOX + BACKHAB_LV2_LENGTH
+
+ Y_BOTTOM_BAR = Y_BOTTOM_BACKHAB_BOX - LENGTH_BARS
+
+ set_color(cr, palette["BLACK"])
+ cr.rectangle(BACKWALL_X, Y_TOP_BACKHAB_BOX, WIDTH_BACKHAB,
+ BACKHAB_LV2_LENGTH)
+ cr.rectangle(BACKWALL_X, Y_LV3_BOX, WIDTH_BACKHAB, BACKHAB_LV3_LENGTH)
+ cr.rectangle(BACKWALL_X, Y_BOTTOM_BACKHAB_BOX, WIDTH_BACKHAB,
+ BACKHAB_LV2_LENGTH)
+ cr.rectangle(X_LV1_BOX, Y_BASE, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
+ cr.rectangle(X_LV1_BOX, Y_BOTTOM_LV1_BOX, WIDTH_LV1_BOX, LENGTH_LV1_BOX)
+ cr.rectangle(X_RAMP, Y_TOP_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
+ cr.rectangle(X_MIDDLE_RAMP, Y_MIDDLE_RAMP, WIDTH_MIDDLE_RAMP,
+ LENGTH_MIDDLE_RAMP)
+ cr.rectangle(X_RAMP, Y_BOTTOM_RAMP, WIDTH_TOP_RAMP, LENGTH_TOP_RAMP)
+ cr.rectangle(X_BARS, Y_TOP_BAR, WIDTH_BARS, LENGTH_BARS)
+ cr.rectangle(X_BARS, Y_BOTTOM_BAR, WIDTH_BARS, LENGTH_BARS)
+ cr.stroke()
+
+ cr.set_line_join(cairo.LINE_JOIN_ROUND)
+
+ cr.stroke()
+
+ #draw 0, 0
+ set_color(cr, palette["BLACK"])
+ cr.move_to(X_TRANSLATE, Y_TRANSLATE)
+ cr.line_to(X_TRANSLATE, Y_TRANSLATE + mToPx(8.2296 / 2.0))
+ cr.move_to(X_TRANSLATE, Y_TRANSLATE)
+ cr.line_to(X_TRANSLATE, Y_TRANSLATE + mToPx(-8.2296 / 2.0))
+ cr.move_to(X_TRANSLATE, Y_TRANSLATE)
+ cr.line_to(X_TRANSLATE + mToPx(8.2296), Y_TRANSLATE)
+
+ cr.stroke()
+
+
+def draw_rockets(cr):
+ # BASE Constants
+ X_BASE = 0 + X_TRANSLATE + mToPx(2.41568)
+ Y_BASE = 0 + Y_TRANSLATE #mToPx(4.129151)
+
+ near_side_rocket_center = [
+ X_BASE + mToPx((2.89973 + 3.15642) / 2.0), Y_BASE + mToPx(
+ (3.86305 + 3.39548) / 2.0)
+ ]
+ middle_rocket_center = [
+ X_BASE + mToPx((3.15642 + 3.6347) / 2.0), Y_BASE + mToPx(
+ (3.39548 + 3.392380) / 2.0)
+ ]
+ far_side_rocket_center = [
+ X_BASE + mToPx((3.63473 + 3.89984) / 2.0), Y_BASE + mToPx(
+ (3.39238 + 3.86305) / 2.0)
+ ]
+
+ cr.move_to(near_side_rocket_center[0], near_side_rocket_center[1])
+ cr.line_to(near_side_rocket_center[0] - 0.4 * mToPx(0.866),
+ near_side_rocket_center[1] - 0.4 * mToPx(0.5))
+
+ cr.move_to(middle_rocket_center[0], middle_rocket_center[1])
+ cr.line_to(middle_rocket_center[0], middle_rocket_center[1] - mToPx(0.4))
+
+ cr.move_to(far_side_rocket_center[0], far_side_rocket_center[1])
+ cr.line_to(far_side_rocket_center[0] + 0.4 * mToPx(0.866),
+ far_side_rocket_center[1] - 0.4 * mToPx(0.5))
+
+ print("FAR SIDE ROCKET")
+ #print(far_side_rocket_center)
+ near_side_rocket_center = [
+ X_BASE + mToPx((2.89973 + 3.15642) / 2.0), Y_BASE - mToPx(
+ (3.86305 + 3.39548) / 2.0)
+ ]
+ middle_rocket_center = [
+ X_BASE + mToPx((3.15642 + 3.6347) / 2.0), Y_BASE - mToPx(
+ (3.39548 + 3.392380) / 2.0)
+ ]
+ far_side_rocket_center = [
+ X_BASE + mToPx((3.63473 + 3.89984) / 2.0), Y_BASE - mToPx(
+ (3.39238 + 3.86305) / 2.0)
+ ]
+
+ cr.move_to(near_side_rocket_center[0], near_side_rocket_center[1])
+ cr.line_to(near_side_rocket_center[0] - 0.4 * mToPx(0.866),
+ near_side_rocket_center[1] + 0.4 * mToPx(0.5))
+
+ cr.move_to(middle_rocket_center[0], middle_rocket_center[1])
+ cr.line_to(middle_rocket_center[0], middle_rocket_center[1] + mToPx(0.4))
+
+ cr.move_to(far_side_rocket_center[0], far_side_rocket_center[1])
+ cr.line_to(far_side_rocket_center[0] + 0.4 * mToPx(0.866),
+ far_side_rocket_center[1] + 0.4 * mToPx(0.5))
+
+ X_BASE = 0 + X_TRANSLATE #mToPx(2.41568)
+ Y_BASE = 0 + Y_TRANSLATE #mToPx(4.129151)
+
+ # Leftmost Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
+
+ # Top Line
+ cr.move_to(X_BASE + mToPx(3.15642), Y_BASE + mToPx(3.39548))
+ cr.line_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
+
+ #Rightmost Line
+ cr.move_to(X_BASE + mToPx(3.63473), Y_BASE + mToPx(3.39238))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
+
+ #Back Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE + mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE + mToPx(3.86305))
+
+ # Leftmost Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
+
+ # Top Line
+ cr.move_to(X_BASE + mToPx(3.15642), Y_BASE - mToPx(3.39548))
+ cr.line_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
+
+ #Rightmost Line
+ cr.move_to(X_BASE + mToPx(3.63473), Y_BASE - mToPx(3.39238))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
+
+ #Back Line
+ cr.move_to(X_BASE + mToPx(2.89973), Y_BASE - mToPx(3.86305))
+ cr.line_to(X_BASE + mToPx(3.89984), Y_BASE - mToPx(3.86305))
+
+ cr.stroke()
+
+
+def draw_cargo_ship(cr):
+ # BASE Constants
+
+ X_BASE = X_TRANSLATE + mToPx(5.59435)
+ Y_BASE = 0 + Y_TRANSLATE #mToPx(4.129151)
+
+ FRONT_PEG_DELTA_Y = mToPx(0.276352)
+ cr.move_to(X_BASE, Y_BASE + FRONT_PEG_DELTA_Y)
+ cr.line_to(X_BASE - mToPx(0.4), Y_BASE + FRONT_PEG_DELTA_Y)
+
+ cr.move_to(X_BASE, Y_BASE - FRONT_PEG_DELTA_Y)
+ cr.line_to(X_BASE - mToPx(0.4), Y_BASE - FRONT_PEG_DELTA_Y)
+
+ SIDE_PEG_Y = mToPx(1.41605 / 2.0)
+ SIDE_PEG_X = X_BASE + mToPx(1.148842)
+ SIDE_PEG_DX = mToPx(0.55245)
+
+ cr.move_to(SIDE_PEG_X, SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X, SIDE_PEG_Y + mToPx(0.4))
+
+ cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + SIDE_PEG_DX, SIDE_PEG_Y + mToPx(0.4))
+
+ cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, SIDE_PEG_Y + mToPx(0.4))
+
+ cr.move_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X, -1.0 * SIDE_PEG_Y - mToPx(0.4))
+
+ cr.move_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(0.4))
+
+ cr.move_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y)
+ cr.line_to(SIDE_PEG_X + 2.0 * SIDE_PEG_DX, -1.0 * SIDE_PEG_Y - mToPx(0.4))
+
+ cr.rectangle(X_BASE, Y_BASE - mToPx(1.41605 / 2.0), mToPx(2.43205),
+ mToPx(1.41605))
+
+ X_BASE = X_TRANSLATE + mToPx(2.41568)
+ Y_BASE = 0 + Y_TRANSLATE #mToPx(4.129151)
+
+ cr.rectangle(X_BASE + mToPx(3.15912), Y_BASE - mToPx(0.72326),
+ mToPx(2.43205), mToPx(1.41605))
+
+ cr.stroke()
diff --git a/frc971/control_loops/python/spline_graph.py b/frc971/control_loops/python/spline_graph.py
new file mode 100755
index 0000000..2fd7964
--- /dev/null
+++ b/frc971/control_loops/python/spline_graph.py
@@ -0,0 +1,100 @@
+#!/usr/bin/python3
+import gi
+from path_edit import *
+import numpy as np
+gi.require_version('Gtk', '3.0')
+from gi.repository import Gdk, Gtk, GLib
+import cairo
+
+class GridWindow(Gtk.Window):
+ def method_connect(self, event, cb):
+ def handler(obj, *args):
+ cb(*args)
+
+ print("Method_connect ran")
+ self.connect(event, handler)
+
+ def mouse_move(self, event):
+ #Changes event.x and event.y to be relative to the center.
+ x = event.x - self.drawing_area.window_shape[0] / 2
+ y = self.drawing_area.window_shape[1] / 2 - event.y
+ scale = self.drawing_area.get_current_scale()
+ event.x = x / scale + self.drawing_area.center[0]
+ event.y = y / scale + self.drawing_area.center[1]
+ self.drawing_area.mouse_move(event)
+ self.queue_draw()
+
+ def button_press(self, event):
+ print("button press activated")
+ o_x = event.x
+ o_y = event.y
+ x = event.x - self.drawing_area.window_shape[0] / 2
+ y = self.drawing_area.window_shape[1] / 2 - event.y
+ scale = 2 * self.drawing_area.get_current_scale()
+ event.x = x / scale + self.drawing_area.center[0]
+ event.y = y / scale + self.drawing_area.center[1]
+ self.drawing_area.do_button_press(event)
+ event.x = o_x
+ event.y = o_y
+
+ def key_press(self, event):
+ print("key press activated")
+ self.drawing_area.do_key_press(event, self.file_name_box.get_text())
+ self.queue_draw()
+
+ def configure(self, event):
+ print("configure activated")
+ self.drawing_area.window_shape = (event.width, event.height)
+
+ # handle submitting a constraint
+ def on_submit_click(self, widget):
+ self.drawing_area.inConstraint = int(self.constraint_box.get_text())
+ self.drawing_area.inValue = int(self.value_box.get_text())
+
+ def __init__(self):
+ Gtk.Window.__init__(self)
+
+ self.set_default_size(1.5 * SCREEN_SIZE, SCREEN_SIZE)
+
+ flowBox = Gtk.FlowBox()
+ flowBox.set_valign(Gtk.Align.START)
+ flowBox.set_selection_mode(Gtk.SelectionMode.NONE)
+
+ flowBox.set_valign(Gtk.Align.START)
+
+ self.add(flowBox)
+
+ container = Gtk.Fixed()
+ flowBox.add(container)
+
+ self.eventBox = Gtk.EventBox()
+ container.add(self.eventBox)
+
+ self.eventBox.set_events(Gdk.EventMask.BUTTON_PRESS_MASK
+ | Gdk.EventMask.BUTTON_RELEASE_MASK
+ | Gdk.EventMask.POINTER_MOTION_MASK
+ | Gdk.EventMask.SCROLL_MASK
+ | Gdk.EventMask.KEY_PRESS_MASK)
+
+ #add the graph box
+ self.drawing_area = GTK_Widget()
+ self.eventBox.add(self.drawing_area)
+
+ self.method_connect("key-release-event", self.key_press)
+ self.method_connect("button-release-event", self.button_press)
+ self.method_connect("configure-event", self.configure)
+ self.method_connect("motion_notify_event", self.mouse_move)
+
+ self.file_name_box = Gtk.Entry()
+ self.file_name_box.set_size_request(200, 40)
+
+ self.file_name_box.set_text("File")
+ self.file_name_box.set_editable(True)
+
+ container.put(self.file_name_box, 0, 0)
+
+ self.show_all()
+
+
+window = GridWindow()
+RunApp()
diff --git a/frc971/control_loops/python/spline_graph_usage.txt b/frc971/control_loops/python/spline_graph_usage.txt
new file mode 100644
index 0000000..a698298
--- /dev/null
+++ b/frc971/control_loops/python/spline_graph_usage.txt
@@ -0,0 +1,22 @@
+Spline Graph Usage
+
+Run with: bazel run spline_graph -c opt "YOUR SCREEN SIZE"
+
+On Start Place down first 6 points
+Then you go into editing mode, where you can drag your points around
+Then to create another spline, press p and the first three points will be autogenerated
+
+To export type in your file in the top right textfield ex.test.json
+It will be exported to spline_jsons/YOURFILE
+
+Notes:
+If importing gi, gi requires #!/usr/bin/python3 be at the top of the file
+
+This was built on top of BaseWindow
+
+Known Bugs:
+libspline throws errors about viable friction limits and viable voltage limits not overlapping
+bazel run lib_spline_test -c opt
+John commented those lines out
+
+Upon closing, reopens window
diff --git a/frc971/control_loops/python/spline_jsons/test.json b/frc971/control_loops/python/spline_jsons/test.json
new file mode 100644
index 0000000..09737e0
--- /dev/null
+++ b/frc971/control_loops/python/spline_jsons/test.json
@@ -0,0 +1 @@
+[[[1.6680953371273437, -3.4989921261100347], [1.8841223685193285, -3.3960504631707944], [2.0281410171518672, -3.272708402385707], [2.192647314562578, -3.1286520914802223], [2.3984302077132025, -3.0360669258966317], [2.5936689198479943, -2.912724865111545]], [[2.5936689198479943, -2.912724865111545], [2.788907631982786, -2.789382804326458], [2.973602163101745, -2.635283848339876], [3.190230535451773, -2.5732992795250538], [3.396013428602397, -2.511627935280236], [3.6117404172199103, -2.4705139150185405]]]
\ No newline at end of file
diff --git a/frc971/control_loops/python/spline_writer.py b/frc971/control_loops/python/spline_writer.py
new file mode 100644
index 0000000..df622f3
--- /dev/null
+++ b/frc971/control_loops/python/spline_writer.py
@@ -0,0 +1,72 @@
+import os
+import numpy as np
+
+
+class SplineWriter(object):
+ def __init__(self, namespaces=None, filename="auto_splines.cc"):
+ if namespaces:
+ self._namespaces = namespaces
+ else:
+ self._namespaces = ['frc971', 'autonomous']
+
+ self._namespace_start = '\n'.join(
+ ['namespace %s {' % name for name in self._namespaces])
+
+ self._namespace_end = '\n'.join([
+ '} // namespace %s' % name for name in reversed(self._namespaces)
+ ])
+
+ self.filename_ = filename
+
+ def make_string_list(self, list):
+ string = "{{"
+ for i in range(0, len(list)):
+ if i == len(list) - 1:
+ string += str(list[i])
+ else:
+ string += str(list[i]) + ", "
+ return string + "}}"
+
+ def Write(self, spline_name, spline_idx, control_points):
+ """Writes the cc file to the file named cc_file."""
+ xs = control_points[:, 0]
+ ys = control_points[:, 1]
+ spline_count = (len(xs) - 6) / 5 + 1
+ with open(self.filename_, 'a') as fd:
+ fd.write(self._namespace_start)
+ #write the name
+ fd.write("\n\n::frc971::MultiSpline " + spline_name + "() {\n")
+ # write the objs, at the moment assumes a single constraint, needs fixing
+ fd.write(
+ "\t::frc971::MultiSpline spline;\n\t::frc971::Constraint constraints;\n"
+ )
+ fd.write(
+ "\tconstraints.constraint_type = 0;\n\tconstraints.value = 0;\n"
+ )
+ fd.write(
+ "\tconstraints.start_distance = 0;\n\tconstraints.end_distance = 0;\n"
+ )
+ fd.write('\n')
+ fd.write("\tspline.spline_idx = " + str(spline_idx) +
+ ";\n\tspline.spline_count = " + str(spline_count) + ";\n")
+ fd.write("\tspline.spline_x = " + self.make_string_list(xs) +
+ ";\n\tspline.spline_y = " + self.make_string_list(ys) +
+ ";\n")
+ fd.write(
+ "\tspline.constraints = {{constraints}};\n\treturn spline;\n")
+ fd.write("}")
+ fd.write("\n\n")
+ fd.write(self._namespace_end)
+ fd.write("\n\n")
+
+
+def main():
+ writer = SplineWriter()
+ points = np.array([[1.0, 2.0], [3.0, 4.0], [5.0, 6.0], [7.0, 8.0],
+ [9.0, 10.0], [11.0, 12.0]])
+ spline_name = "test_spline"
+ spline_idx = 1
+ writer.Write(spline_name, spline_idx, points)
+
+
+main()
diff --git a/third_party/flatbuffers/src/idl_gen_js_ts.cpp b/third_party/flatbuffers/src/idl_gen_js_ts.cpp
index be0a205..cfb0cb6 100644
--- a/third_party/flatbuffers/src/idl_gen_js_ts.cpp
+++ b/third_party/flatbuffers/src/idl_gen_js_ts.cpp
@@ -153,10 +153,9 @@
imported_files.emplace(file.first);
}
- code += "export namespace " + file.second.target_namespace + " { \n";
+ //code += "export namespace " + file.second.target_namespace + " { \n";
code += "export import " + file.second.symbol + " = ";
- code += GenFileNamespacePrefix(file.first) + "." +
- file.second.source_namespace + "." + file.second.symbol +
+ code += file.second.symbol +
"; }\n";
}
}
@@ -334,7 +333,7 @@
std::string ns = GetNameSpace(enum_def);
std::string enum_def_name = enum_def.name + (reverse ? "Name" : "");
if (lang_.language == IDLOptions::kTs) {
- if (!ns.empty()) { code += "export namespace " + ns + "{\n"; }
+ //if (!ns.empty()) { code += "export namespace " + ns + "{\n"; }
code += "export enum " + enum_def.name + "{\n";
} else {
if (enum_def.defined_namespace->components.empty()) {
@@ -348,7 +347,7 @@
exports += "this." + enum_def_name + " = " + enum_def_name + ";\n";
}
}
- code += WrapInNameSpace(enum_def) + (reverse ? "Name" : "") + " = {\n";
+ code += enum_def.name + (reverse ? "Name" : "") + " = {\n";
}
for (auto it = enum_def.Vals().begin(); it != enum_def.Vals().end(); ++it) {
auto &ev = **it;
@@ -416,7 +415,7 @@
if (type.base_type == BASE_TYPE_BOOL) { getter = "!!" + getter; }
if (type.enum_def) {
getter = "/** " +
- GenTypeAnnotation(kType, WrapInNameSpace(*type.enum_def), "",
+ GenTypeAnnotation(kType, type.enum_def->name, "",
false) +
" */ (" + getter + ")";
}
@@ -433,15 +432,15 @@
if (value.type.enum_def) {
if (auto val = value.type.enum_def->FindByValue(value.constant)) {
if (lang_.language == IDLOptions::kTs) {
- return GenPrefixedTypeName(WrapInNameSpace(*value.type.enum_def),
+ return GenPrefixedTypeName(value.type.enum_def->name,
value.type.enum_def->file) +
"." + val->name;
} else {
- return WrapInNameSpace(*value.type.enum_def) + "." + val->name;
+ return value.type.enum_def->name + "." + val->name;
}
} else {
return "/** " +
- GenTypeAnnotation(kType, WrapInNameSpace(*value.type.enum_def),
+ GenTypeAnnotation(kType, value.type.enum_def->name,
"", false) +
"} */ (" + value.constant + ")";
}
@@ -473,7 +472,7 @@
if (type.base_type == BASE_TYPE_STRING) {
name = "string|Uint8Array";
} else {
- name = WrapInNameSpace(*type.struct_def);
+ name = type.struct_def->name;
}
return (allowNull) ? (name + "|null") : (name);
}
@@ -485,7 +484,7 @@
case BASE_TYPE_ULONG: return "flatbuffers.Long";
default:
if (IsScalar(type.base_type)) {
- if (type.enum_def) { return WrapInNameSpace(*type.enum_def); }
+ if (type.enum_def) { return type.enum_def->name; }
return "number";
}
return "flatbuffers.Offset";
@@ -675,9 +674,9 @@
if (lang_.language == IDLOptions::kTs) {
object_name = struct_def.name;
GenDocComment(struct_def.doc_comment, code_ptr, "@constructor");
- if (!object_namespace.empty()) {
- code += "export namespace " + object_namespace + "{\n";
- }
+ //if (!object_namespace.empty()) {
+ // code += "export namespace " + object_namespace + "{\n";
+ //}
code += "export class " + struct_def.name;
code += " {\n";
if (lang_.language != IDLOptions::kTs) {
@@ -695,7 +694,7 @@
code += " bb_pos:number = 0;\n";
} else {
bool isStatement = struct_def.defined_namespace->components.empty();
- object_name = WrapInNameSpace(struct_def);
+ object_name = struct_def.name;
GenDocComment(struct_def.doc_comment, code_ptr, "@constructor");
if (isStatement) {
if (parser_.opts.use_goog_js_export_format) {
@@ -851,7 +850,7 @@
else {
switch (field.value.type.base_type) {
case BASE_TYPE_STRUCT: {
- auto type = WrapInNameSpace(*field.value.type.struct_def);
+ auto type = field.value.type.struct_def->name;
GenDocComment(
field.doc_comment, code_ptr,
GenTypeAnnotation(kParam, type + "=", "obj") +
@@ -978,7 +977,7 @@
if (field.value.type.enum_def) {
code += "/** " +
GenTypeAnnotation(
- kType, WrapInNameSpace(*field.value.type.enum_def),
+ kType, field.value.type.enum_def->name,
"", false) +
" */ (" + field.value.constant + ")";
} else {
@@ -1359,9 +1358,9 @@
}
if (lang_.language == IDLOptions::kTs) {
- if (!object_namespace.empty()) {
- code += "}\n";
- }
+ //if (!object_namespace.empty()) {
+ // code += "}\n";
+ //}
code += "}\n";
}
}
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.cc b/y2019/control_loops/drivetrain/event_loop_localizer.cc
index c436232..16cd723 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.cc
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.cc
@@ -58,17 +58,17 @@
localizer_.ResetInitialState(now, state, newP);
}
-void EventLoopLocalizer::Update(
- const ::Eigen::Matrix<double, 2, 1> &U,
- ::aos::monotonic_clock::time_point now, double left_encoder,
- double right_encoder, double gyro_rate,
- double /*longitudinal_accelerometer*/) {
+void EventLoopLocalizer::Update(const ::Eigen::Matrix<double, 2, 1> &U,
+ ::aos::monotonic_clock::time_point now,
+ double left_encoder, double right_encoder,
+ double gyro_rate,
+ const Eigen::Vector3d &accel) {
AOS_CHECK(U.allFinite());
AOS_CHECK(::std::isfinite(left_encoder));
AOS_CHECK(::std::isfinite(right_encoder));
AOS_CHECK(::std::isfinite(gyro_rate));
localizer_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U,
- now);
+ accel, now);
while (frame_fetcher_.FetchNext()) {
HandleFrame(frame_fetcher_.get());
}
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.h b/y2019/control_loops/drivetrain/event_loop_localizer.h
index 83b8e79..5305b8d 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.h
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.h
@@ -60,32 +60,9 @@
void Update(const ::Eigen::Matrix<double, 2, 1> &U,
::aos::monotonic_clock::time_point now, double left_encoder,
double right_encoder, double gyro_rate,
- double /*longitudinal_accelerometer*/) override;
+ const Eigen::Vector3d &accel) override;
- double x() const override {
- return localizer_.X_hat(StateIdx::kX); }
- double y() const override {
- return localizer_.X_hat(StateIdx::kY); }
- double theta() const override {
- return localizer_.X_hat(StateIdx::kTheta); }
- double left_encoder() const override {
- return localizer_.X_hat(StateIdx::kLeftEncoder);
- }
- double right_encoder() const override {
- return localizer_.X_hat(StateIdx::kRightEncoder);
- }
- double left_velocity() const override {
- return localizer_.X_hat(StateIdx::kLeftVelocity);
- }
- double right_velocity() const override {
- return localizer_.X_hat(StateIdx::kRightVelocity);
- }
- double left_voltage_error() const override {
- return localizer_.X_hat(StateIdx::kLeftVoltageError);
- }
- double right_voltage_error() const override {
- return localizer_.X_hat(StateIdx::kRightVoltageError);
- }
+ Localizer::State Xhat() const override { return localizer_.X_hat(); }
TargetSelector *target_selector() override {
return &target_selector_;
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index 9b237f4..1459248 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -111,8 +111,7 @@
const Eigen::Matrix<double, 5, 1> true_state = drivetrain_plant_.state();
EXPECT_NEAR(localizer_.x(), true_state(0, 0), eps);
EXPECT_NEAR(localizer_.y(), true_state(1, 0), eps);
- // TODO(james): Uncomment this.
- //EXPECT_NEAR(localizer_.theta(), true_state(2, 0), 2.0 * eps);
+ EXPECT_NEAR(localizer_.theta(), true_state(2, 0), eps);
EXPECT_NEAR(localizer_.left_velocity(), true_state(3, 0), eps);
EXPECT_NEAR(localizer_.right_velocity(), true_state(4, 0), eps);
}
@@ -290,8 +289,7 @@
// Everything but X-value should be correct:
EXPECT_NEAR(true_state.x(), localizer_.x() + 0.05, 1e-5);
EXPECT_NEAR(true_state.y(), localizer_.y(), 1e-5);
- // TODO(james): Uncomment this.
- //EXPECT_NEAR(true_state(2, 0), localizer_.theta(), 1e-3);
+ EXPECT_NEAR(true_state(2, 0), localizer_.theta(), 1e-3);
EXPECT_NEAR(true_state(3, 0), localizer_.left_velocity(), 1e-4);
EXPECT_NEAR(true_state(4, 0), localizer_.right_velocity(), 1e-4);
}
@@ -383,10 +381,9 @@
VerifyEstimatorAccurate(0.2);
// Due to the fact that we aren't modulating the throttle, we don't try to hit
// the target exactly. Instead, just run slightly past the target:
- // TODO(james): Uncomment this.
- //EXPECT_LT(
- // ::std::abs(::aos::math::DiffAngle(M_PI, drivetrain_plant_.state()(2, 0))),
- // 0.5);
+ EXPECT_LT(
+ ::std::abs(::aos::math::DiffAngle(M_PI, drivetrain_plant_.state()(2, 0))),
+ 0.5);
EXPECT_GT(HPSlotLeft().abs_pos().x(), drivetrain_plant_.state().x());
EXPECT_NEAR(HPSlotLeft().abs_pos().y(), drivetrain_plant_.state().y(), 0.2);
}
diff --git a/y2019/control_loops/drivetrain/localizer_test.cc b/y2019/control_loops/drivetrain/localizer_test.cc
index aa7f22f..7b25ca5 100644
--- a/y2019/control_loops/drivetrain/localizer_test.cc
+++ b/y2019/control_loops/drivetrain/localizer_test.cc
@@ -142,9 +142,6 @@
}
void SetUp() {
- // Turn on -v 1
- FLAGS_v = std::max(FLAGS_v, 1);
-
flatbuffers::DetachedBuffer goal_buffer;
{
flatbuffers::FlatBufferBuilder fbb;
@@ -315,8 +312,12 @@
}
// The differential equation for the dynamics of the system.
TestLocalizer::State DiffEq(const TestLocalizer::State &X,
- const TestLocalizer::Input &U) {
- return localizer_.DiffEq(X, U);
+ const Eigen::Vector2d &voltage) {
+ TestLocalizer::Input U;
+ U.setZero();
+ U(0) = voltage(0);
+ U(1) = voltage(1);
+ return localizer_.DiffEq(X, U, true);
}
Field field_;
@@ -433,7 +434,7 @@
output.left_voltage = 0;
output.right_voltage = 0;
spline_drivetrain_.SetOutput(&output);
- TestLocalizer::Input U(output.left_voltage, output.right_voltage);
+ Eigen::Vector2d U(output.left_voltage, output.right_voltage);
const ::Eigen::Matrix<double, 5, 1> goal_state =
spline_drivetrain_.CurrentGoalState();
@@ -454,7 +455,7 @@
U(1, 0) = ::std::max(::std::min(U(1, 0), 12.0), -12.0);
state = ::frc971::control_loops::RungeKuttaU(
- [this](const ::Eigen::Matrix<double, 10, 1> &X,
+ [this](const ::Eigen::Matrix<double, 12, 1> &X,
const ::Eigen::Matrix<double, 2, 1> &U) { return DiffEq(X, U); },
state, U, ::aos::time::DurationInSeconds(dt_config_.dt));
@@ -462,6 +463,8 @@
// interest here are that we (a) stop adding disturbances at the very end of
// the trajectory, to allow us to actually converge to the goal, and (b)
// scale disturbances by the corruent velocity.
+ // TODO(james): Figure out how to persist good accelerometer values through
+ // the disturbances.
if (GetParam().disturb && i % 75 == 0) {
// Scale the disturbance so that when we have near-zero velocity we don't
// have much disturbance.
@@ -470,7 +473,8 @@
::std::pow(state(StateIdx::kRightVelocity, 0), 2)) /
3.0);
TestLocalizer::State disturbance;
- disturbance << 0.02, 0.02, 0.001, 0.03, 0.02, 0.0, 0.0, 0.0, 0.0, 0.0;
+ disturbance << 0.02, 0.02, 0.001, 0.03, 0.02, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0;
disturbance *= disturbance_scale;
state += disturbance;
}
@@ -482,13 +486,18 @@
const double left_enc = state(StateIdx::kLeftEncoder, 0);
const double right_enc = state(StateIdx::kRightEncoder, 0);
- const double gyro = (state(StateIdx::kRightVelocity, 0) -
- state(StateIdx::kLeftVelocity, 0)) /
+ const double gyro = (state(StateIdx::kRightVelocity) -
+ state(StateIdx::kLeftVelocity)) /
dt_config_.robot_radius / 2.0;
+ const TestLocalizer::State xdot = DiffEq(state, U);
+ const Eigen::Vector3d accel(
+ localizer_.CalcLongitudinalVelocity(xdot) -
+ gyro * state(StateIdx::kLateralVelocity),
+ gyro * localizer_.CalcLongitudinalVelocity(state), 1.0);
localizer_.UpdateEncodersAndGyro(left_enc + Normal(encoder_noise_),
right_enc + Normal(encoder_noise_),
- gyro + Normal(gyro_noise_), U, t);
+ gyro + Normal(gyro_noise_), U, accel, t);
for (size_t cam_idx = 0; cam_idx < camera_queues.size(); ++cam_idx) {
auto &camera_queue = camera_queues[cam_idx];
@@ -551,14 +560,14 @@
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
(TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
(TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/false,
/*disturb=*/false,
- /*estimate_tolerance=*/1e-5,
+ /*estimate_tolerance=*/1e-2,
/*goal_tolerance=*/2e-2,
}),
// Checks "perfect" estimation, but start off the spline and check
@@ -567,14 +576,14 @@
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
(TestLocalizer::State() << 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
(TestLocalizer::State() << 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/false,
/*disturb=*/false,
- /*estimate_tolerance=*/1e-5,
+ /*estimate_tolerance=*/1e-2,
/*goal_tolerance=*/2e-2,
}),
// Repeats perfect scenario, but add sensor noise.
@@ -582,10 +591,10 @@
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
(TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
(TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/true,
/*disturb=*/false,
@@ -597,14 +606,14 @@
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
(TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
(TestLocalizer::State() << 0.1, -5.1, -0.01, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/false,
/*disturb=*/false,
- /*estimate_tolerance=*/1e-4,
+ /*estimate_tolerance=*/1e-2,
/*goal_tolerance=*/2e-2,
}),
// Repeats perfect scenario, but add voltage + angular errors:
@@ -612,14 +621,14 @@
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
(TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0,
- 0.5, 0.02)
+ 0.5, 0.02, 0.0, 0.0)
.finished(),
(TestLocalizer::State() << 0.1, -5.1, -0.01, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/false,
/*disturb=*/false,
- /*estimate_tolerance=*/1e-4,
+ /*estimate_tolerance=*/1e-2,
/*goal_tolerance=*/2e-2,
}),
// Add disturbances while we are driving:
@@ -627,14 +636,14 @@
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
(TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
(TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/false,
/*disturb=*/true,
- /*estimate_tolerance=*/3e-2,
+ /*estimate_tolerance=*/4e-2,
/*goal_tolerance=*/0.15,
}),
// Add noise and some initial error in addition:
@@ -642,15 +651,15 @@
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
(TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
(TestLocalizer::State() << 0.1, -5.1, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/true,
/*disturb=*/true,
- /*estimate_tolerance=*/0.2,
- /*goal_tolerance=*/0.5,
+ /*estimate_tolerance=*/0.5,
+ /*goal_tolerance=*/1.0,
}),
// Try another spline, just in case the one I was using is special for
// some reason; this path will also go straight up to a target, to
@@ -659,10 +668,10 @@
/*control_pts_x=*/{{0.5, 3.5, 4.0, 8.0, 11.0, 10.2}},
/*control_pts_y=*/{{1.0, 1.0, -3.0, -2.0, -3.5, -3.65}},
(TestLocalizer::State() << 0.6, 1.01, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
(TestLocalizer::State() << 0.5, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0)
+ 0.0, 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/true,
/*disturb=*/false,
diff --git a/y2020/BUILD b/y2020/BUILD
index 9a233d4..f91f532 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -30,7 +30,6 @@
"//aos/mutex",
"//aos/network:team_number",
"//frc971:constants",
- "//frc971/control_loops:pose",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//y2020/control_loops/drivetrain:polydrivetrain_plants",
"//y2020/control_loops/superstructure/accelerator:accelerator_plants",
diff --git a/y2020/constants.h b/y2020/constants.h
index 4713072..7fbb704 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -7,7 +7,6 @@
#include <array>
#include "frc971/constants.h"
-#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2020/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2020/control_loops/superstructure/accelerator/accelerator_plant.h"
diff --git a/y2020/control_loops/drivetrain/BUILD b/y2020/control_loops/drivetrain/BUILD
index 9abb2a1..de669d6 100644
--- a/y2020/control_loops/drivetrain/BUILD
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -1,4 +1,5 @@
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos:config.bzl", "aos_config")
load("//tools/build_rules:select.bzl", "compiler_select", "cpu_select")
genrule(
@@ -53,6 +54,21 @@
)
cc_library(
+ name = "localizer",
+ srcs = ["localizer.cc"],
+ hdrs = ["localizer.h"],
+ deps = [
+ "//aos/containers:ring_buffer",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/drivetrain:hybrid_ekf",
+ "//frc971/control_loops/drivetrain:localizer",
+ "//y2020:constants",
+ "//y2020/control_loops/superstructure:superstructure_status_fbs",
+ "//y2020/vision/sift:sift_fbs",
+ ],
+)
+
+cc_library(
name = "drivetrain_base",
srcs = [
"drivetrain_base.cc",
@@ -82,6 +98,54 @@
],
)
+aos_config(
+ name = "simulation_config",
+ src = "drivetrain_simulation_config.json",
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops/drivetrain:simulation_channels",
+ "//y2020:config",
+ ],
+)
+
+cc_test(
+ name = "localizer_test",
+ srcs = ["localizer_test.cc"],
+ data = [":simulation_config.json"],
+ deps = [
+ ":drivetrain_base",
+ ":localizer",
+ "//aos/controls:control_loop_test",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:logger",
+ "//aos/network:team_number",
+ "//frc971/control_loops:team_number_test_environment",
+ "//frc971/control_loops/drivetrain:drivetrain_lib",
+ "//frc971/control_loops/drivetrain:drivetrain_test_lib",
+ ],
+)
+
+cc_test(
+ name = "drivetrain_replay_test",
+ srcs = ["drivetrain_replay_test.cc"],
+ data = [
+ "//y2020:config.json",
+ "@drivetrain_replay//file:spinning_wheels_while_still.bfbs",
+ ],
+ deps = [
+ ":drivetrain_base",
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:logger",
+ "//aos/testing:googletest",
+ "//frc971/control_loops/drivetrain:drivetrain_lib",
+ "@com_github_gflags_gflags//:gflags",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
cc_binary(
name = "drivetrain_replay",
srcs = ["drivetrain_replay.cc"],
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay_test.cc b/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
new file mode 100644
index 0000000..6933a51
--- /dev/null
+++ b/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
@@ -0,0 +1,99 @@
+// This file serves to test replaying the localizer over existing logfiles to
+// check for regressions. Currently, it just uses a single logfile pulled from
+// running the 2016 robot against a wall and confirming that the X/Y estimate
+// does not change too much.
+//
+// Note that the current logfile test will break once we update the drivetrain
+// config for 2020, since both the gear ratios and IMU transformation wil no
+// longer be valid.
+// TODO(james): Do something about that when the time comes--could just copy
+// the existing drivetrain config into this file and use it directly.
+#include "gtest/gtest.h"
+
+#include "aos/configuration.h"
+#include "aos/events/logging/logger.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/network/team_number.h"
+#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "gflags/gflags.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+
+DEFINE_string(
+ logfile, "external/drivetrain_replay/file/spinning_wheels_while_still.bfbs",
+ "Name of the logfile to read from.");
+DEFINE_string(config, "y2020/config.json",
+ "Name of the config file to replay using.");
+
+namespace y2020 {
+namespace control_loops {
+namespace drivetrain {
+namespace testing {
+
+class DrivetrainReplayTest : public ::testing::Test {
+ public:
+ DrivetrainReplayTest()
+ : config_(aos::configuration::ReadConfig(FLAGS_config)),
+ reader_(FLAGS_logfile, &config_.message()) {
+ aos::network::OverrideTeamNumber(971);
+
+ // TODO(james): Actually enforce not sending on the same buses as the
+ // logfile spews out.
+ reader_.RemapLoggedChannel("/drivetrain",
+ "frc971.control_loops.drivetrain.Status");
+ reader_.RemapLoggedChannel("/drivetrain",
+ "frc971.control_loops.drivetrain.Output");
+ reader_.Register();
+
+ drivetrain_event_loop_ =
+ reader_.event_loop_factory()->MakeEventLoop("drivetrain");
+ drivetrain_event_loop_->SkipTimingReport();
+
+ localizer_ =
+ std::make_unique<frc971::control_loops::drivetrain::DeadReckonEkf>(
+ drivetrain_event_loop_.get(), GetDrivetrainConfig());
+ drivetrain_ =
+ std::make_unique<frc971::control_loops::drivetrain::DrivetrainLoop>(
+ GetDrivetrainConfig(), drivetrain_event_loop_.get(),
+ localizer_.get());
+
+ test_event_loop_ =
+ reader_.event_loop_factory()->MakeEventLoop("drivetrain");
+ status_fetcher_ = test_event_loop_->MakeFetcher<
+ frc971::control_loops::drivetrain::Status>("/drivetrain");
+ }
+
+ const aos::FlatbufferDetachedBuffer<aos::Configuration> config_;
+ aos::logger::LogReader reader_;
+
+ std::unique_ptr<aos::EventLoop> drivetrain_event_loop_;
+ std::unique_ptr<frc971::control_loops::drivetrain::DeadReckonEkf> localizer_;
+ std::unique_ptr<frc971::control_loops::drivetrain::DrivetrainLoop>
+ drivetrain_;
+ std::unique_ptr<aos::EventLoop> test_event_loop_;
+
+ aos::Fetcher<frc971::control_loops::drivetrain::Status> status_fetcher_;
+};
+
+// Tests that we do a good job of trusting the IMU when the wheels are spinning
+// and the actual robot is not moving.
+TEST_F(DrivetrainReplayTest, SpinningWheels) {
+ reader_.event_loop_factory()->Run();
+
+ ASSERT_TRUE(status_fetcher_.Fetch());
+ ASSERT_TRUE(status_fetcher_->has_x());
+ ASSERT_TRUE(status_fetcher_->has_y());
+ ASSERT_TRUE(status_fetcher_->has_theta());
+ EXPECT_LT(std::abs(status_fetcher_->x()), 0.1);
+ // Because the encoders should not be affecting the y or yaw axes, expect a
+ // reasonably precise result (although, since this is a real worl dtest, the
+ // robot probably did actually move be some non-zero amount).
+ EXPECT_LT(std::abs(status_fetcher_->y()), 0.05);
+ EXPECT_LT(std::abs(status_fetcher_->theta()), 0.02);
+}
+
+} // namespace testing
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace y2020
diff --git a/y2020/control_loops/drivetrain/drivetrain_simulation_config.json b/y2020/control_loops/drivetrain/drivetrain_simulation_config.json
new file mode 100644
index 0000000..f4bfbea
--- /dev/null
+++ b/y2020/control_loops/drivetrain/drivetrain_simulation_config.json
@@ -0,0 +1,6 @@
+{
+ "imports": [
+ "../../y2020.json",
+ "../../../frc971/control_loops/drivetrain/drivetrain_simulation_channels.json"
+ ]
+}
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
new file mode 100644
index 0000000..a9e454a
--- /dev/null
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -0,0 +1,182 @@
+#include "y2020/control_loops/drivetrain/localizer.h"
+
+#include "y2020/constants.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace drivetrain {
+
+namespace {
+// Converts a flatbuffer TransformationMatrix to an Eigen matrix. Technically,
+// this should be able to do a single memcpy, but the extra verbosity here seems
+// appropriate.
+Eigen::Matrix<double, 4, 4> FlatbufferToTransformationMatrix(
+ const frc971::vision::sift::TransformationMatrix &flatbuffer) {
+ CHECK_EQ(16u, CHECK_NOTNULL(flatbuffer.data())->size());
+ Eigen::Matrix<double, 4, 4> result;
+ result.setIdentity();
+ for (int row = 0; row < 4; ++row) {
+ for (int col = 0; col < 4; ++col) {
+ result(row, col) = (*flatbuffer.data())[row * 4 + col];
+ }
+ }
+ return result;
+}
+} // namespace
+
+Localizer::Localizer(
+ aos::EventLoop *event_loop,
+ const frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &dt_config)
+ : event_loop_(event_loop), dt_config_(dt_config), ekf_(dt_config) {
+ // TODO(james): This doesn't really need to be a watcher; we could just use a
+ // fetcher for the superstructure status.
+ // This probably should be a Fetcher instead of a Watcher, but this
+ // seems simpler for the time being (although technically it should be
+ // possible to do everything we need to using just a Fetcher without
+ // even maintaining a separate buffer, but that seems overly cute).
+ event_loop_->MakeWatcher("/superstructure",
+ [this](const superstructure::Status &status) {
+ HandleSuperstructureStatus(status);
+ });
+
+ image_fetchers_.emplace_back(
+ event_loop_->MakeFetcher<frc971::vision::sift::ImageMatchResult>(
+ "/camera"));
+
+ target_selector_.set_has_target(false);
+}
+
+void Localizer::HandleSuperstructureStatus(
+ const y2020::control_loops::superstructure::Status &status) {
+ CHECK(status.has_turret());
+ turret_data_.Push({event_loop_->monotonic_now(), status.turret()->position(),
+ status.turret()->velocity()});
+}
+
+Localizer::TurretData Localizer::GetTurretDataForTime(
+ aos::monotonic_clock::time_point time) {
+ if (turret_data_.empty()) {
+ return {};
+ }
+
+ aos::monotonic_clock::duration lowest_time_error =
+ aos::monotonic_clock::duration::max();
+ TurretData best_data_match;
+ for (const auto &sample : turret_data_) {
+ const aos::monotonic_clock::duration time_error =
+ std::chrono::abs(sample.receive_time - time);
+ if (time_error < lowest_time_error) {
+ lowest_time_error = time_error;
+ best_data_match = sample;
+ }
+ }
+ return best_data_match;
+}
+
+void Localizer::Update(const ::Eigen::Matrix<double, 2, 1> &U,
+ aos::monotonic_clock::time_point now,
+ double left_encoder, double right_encoder,
+ double gyro_rate, const Eigen::Vector3d &accel) {
+ for (auto &image_fetcher : image_fetchers_) {
+ while (image_fetcher.FetchNext()) {
+ HandleImageMatch(*image_fetcher);
+ }
+ }
+ ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U, accel,
+ now);
+}
+
+void Localizer::HandleImageMatch(
+ const frc971::vision::sift::ImageMatchResult &result) {
+ // TODO(james): compensate for capture time across multiple nodes correctly.
+ aos::monotonic_clock::time_point capture_time(
+ std::chrono::nanoseconds(result.image_monotonic_timestamp_ns()));
+ CHECK(result.has_camera_calibration());
+ // Per the ImageMatchResult specification, we can actually determine whether
+ // the camera is the turret camera just from the presence of the
+ // turret_extrinsics member.
+ const bool is_turret = result.camera_calibration()->has_turret_extrinsics();
+ const TurretData turret_data = GetTurretDataForTime(capture_time);
+ // Ignore readings when the turret is spinning too fast, on the assumption
+ // that the odds of screwing up the time compensation are higher.
+ // Note that the current number here is chosen pretty arbitrarily--1 rad / sec
+ // seems reasonable, but may be unnecessarily low or high.
+ constexpr double kMaxTurretVelocity = 1.0;
+ if (is_turret && std::abs(turret_data.velocity) > kMaxTurretVelocity) {
+ return;
+ }
+ CHECK(result.camera_calibration()->has_fixed_extrinsics());
+ const Eigen::Matrix<double, 4, 4> fixed_extrinsics =
+ FlatbufferToTransformationMatrix(
+ *result.camera_calibration()->fixed_extrinsics());
+ // Calculate the pose of the camera relative to the robot origin.
+ Eigen::Matrix<double, 4, 4> H_camera_robot = fixed_extrinsics;
+ if (is_turret) {
+ H_camera_robot = H_camera_robot *
+ frc971::control_loops::TransformationMatrixForYaw(
+ turret_data.position) *
+ FlatbufferToTransformationMatrix(
+ *result.camera_calibration()->turret_extrinsics());
+ }
+
+ if (!result.has_camera_poses()) {
+ return;
+ }
+
+ for (const frc971::vision::sift::CameraPose *vision_result :
+ *result.camera_poses()) {
+ if (!vision_result->has_camera_to_target() ||
+ !vision_result->has_field_to_target()) {
+ continue;
+ }
+ const Eigen::Matrix<double, 4, 4> H_target_camera =
+ FlatbufferToTransformationMatrix(*vision_result->camera_to_target());
+ const Eigen::Matrix<double, 4, 4> H_target_field =
+ FlatbufferToTransformationMatrix(*vision_result->field_to_target());
+ // Back out the robot position that is implied by the current camera
+ // reading.
+ const Pose measured_pose(H_target_field *
+ (H_camera_robot * H_target_camera).inverse());
+ const Eigen::Matrix<double, 3, 1> Z(measured_pose.rel_pos().x(),
+ measured_pose.rel_pos().y(),
+ measured_pose.rel_theta());
+ // TODO(james): Figure out how to properly handle calculating the
+ // noise. Currently, the values are deliberately tuned so that image updates
+ // will not be trusted overly much. In theory, we should probably also be
+ // populating some cross-correlation terms.
+ // Note that these are the noise standard deviations (they are squared below
+ // to get variances).
+ Eigen::Matrix<double, 3, 1> noises(1.0, 1.0, 0.1);
+ // Augment the noise by the approximate rotational speed of the
+ // camera. This should help account for the fact that, while we are
+ // spinning, slight timing errors in the camera/turret data will tend to
+ // have mutch more drastic effects on the results.
+ noises *= 1.0 + std::abs((right_velocity() - left_velocity()) /
+ (2.0 * dt_config_.robot_radius) +
+ (is_turret ? turret_data.velocity : 0.0));
+ Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
+ R.diagonal() = noises.cwiseAbs2();
+ Eigen::Matrix<double, HybridEkf::kNOutputs, HybridEkf::kNStates> H;
+ H.setZero();
+ H(0, StateIdx::kX) = 1;
+ H(1, StateIdx::kY) = 1;
+ H(2, StateIdx::kTheta) = 1;
+ ekf_.Correct(Z, nullptr, {}, [H, Z](const State &X, const Input &) {
+ Eigen::Vector3d Zhat = H * X;
+ // In order to deal with wrapping of the
+ // angle, calculate an expected angle that is
+ // in the range (Z(2) - pi, Z(2) + pi].
+ const double angle_error =
+ aos::math::NormalizeAngle(
+ X(StateIdx::kTheta) - Z(2));
+ Zhat(2) = Z(2) + angle_error;
+ return Zhat;
+ },
+ [H](const State &) { return H; }, R, capture_time);
+ }
+}
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace y2020
diff --git a/y2020/control_loops/drivetrain/localizer.h b/y2020/control_loops/drivetrain/localizer.h
new file mode 100644
index 0000000..0b79361
--- /dev/null
+++ b/y2020/control_loops/drivetrain/localizer.h
@@ -0,0 +1,102 @@
+#ifndef Y2020_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
+#define Y2020_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
+
+#include "aos/containers/ring_buffer.h"
+#include "aos/events/event_loop.h"
+#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
+#include "frc971/control_loops/drivetrain/localizer.h"
+#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2020/vision/sift/sift_generated.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace drivetrain {
+
+// This class handles the localization for the 2020 robot. In order to handle
+// camera updates, we get the ImageMatchResult message from the cameras and then
+// project the result onto the 2-D X/Y plane and use the implied robot
+// position/heading from that as the measurement. This is distinct from 2019,
+// when we used a heading/distance/skew measurement update. This is because
+// updating with x/y/theta directly seems to be better conditioned (even if it
+// may not reflect the measurement noise quite as accurately). The poor
+// conditioning seemed to work in 2019, but due to the addition of a couple of
+// velocity offset states that allow us to use the accelerometer more
+// effectively, things started to become unstable.
+class Localizer : public frc971::control_loops::drivetrain::LocalizerInterface {
+ public:
+ typedef frc971::control_loops::TypedPose<double> Pose;
+ typedef frc971::control_loops::drivetrain::HybridEkf<double> HybridEkf;
+ typedef typename HybridEkf::State State;
+ typedef typename HybridEkf::StateIdx StateIdx;
+ typedef typename HybridEkf::StateSquare StateSquare;
+ typedef typename HybridEkf::Input Input;
+ typedef typename HybridEkf::Output Output;
+ Localizer(aos::EventLoop *event_loop,
+ const frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &dt_config);
+ State Xhat() const override { return ekf_.X_hat(); }
+ frc971::control_loops::drivetrain::TrivialTargetSelector *target_selector()
+ override {
+ return &target_selector_;
+ }
+
+ void Update(const ::Eigen::Matrix<double, 2, 1> &U,
+ aos::monotonic_clock::time_point now, double left_encoder,
+ double right_encoder, double gyro_rate,
+ const Eigen::Vector3d &accel) override;
+
+ void Reset(aos::monotonic_clock::time_point t, const State &state) {
+ ekf_.ResetInitialState(t, state, ekf_.P());
+ }
+
+ void ResetPosition(aos::monotonic_clock::time_point t, double x, double y,
+ double theta, double /*theta_override*/,
+ bool /*reset_theta*/) override {
+ const double left_encoder = ekf_.X_hat(StateIdx::kLeftEncoder);
+ const double right_encoder = ekf_.X_hat(StateIdx::kRightEncoder);
+ ekf_.ResetInitialState(t, (Ekf::State() << x, y, theta, left_encoder, 0,
+ right_encoder, 0, 0, 0, 0, 0, 0).finished(),
+ ekf_.P());
+ };
+
+ private:
+ // Storage for a single turret position data point.
+ struct TurretData {
+ aos::monotonic_clock::time_point receive_time =
+ aos::monotonic_clock::min_time;
+ double position = 0.0; // rad
+ double velocity = 0.0; // rad/sec
+ };
+
+ // Processes new image data and updates the EKF.
+ void HandleImageMatch(const frc971::vision::sift::ImageMatchResult &result);
+
+ // Processes the most recent turret position and stores it in the turret_data_
+ // buffer.
+ void HandleSuperstructureStatus(
+ const y2020::control_loops::superstructure::Status &status);
+
+ // Retrieves the turret data closest to the provided time.
+ TurretData GetTurretDataForTime(aos::monotonic_clock::time_point time);
+
+ aos::EventLoop *const event_loop_;
+ const frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
+ HybridEkf ekf_;
+
+ std::vector<aos::Fetcher<frc971::vision::sift::ImageMatchResult>>
+ image_fetchers_;
+
+ // Buffer of recent turret data--this is used so that when we receive a camera
+ // frame from the turret, we can back out what the turret angle was at that
+ // time.
+ aos::RingBuffer<TurretData, 200> turret_data_;
+
+ // Target selector to allow us to satisfy the LocalizerInterface requirements.
+ frc971::control_loops::drivetrain::TrivialTargetSelector target_selector_;
+};
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace y2020
+
+#endif // Y2020_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
new file mode 100644
index 0000000..3be86da
--- /dev/null
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -0,0 +1,497 @@
+#include <queue>
+
+#include "gtest/gtest.h"
+
+#include "aos/controls/control_loop_test.h"
+#include "aos/events/logging/logger.h"
+#include "aos/network/team_number.h"
+#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+#include "y2020/control_loops/drivetrain/localizer.h"
+#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
+
+DEFINE_string(output_file, "",
+ "If set, logs all channels to the provided logfile.");
+
+// This file tests that the full 2020 localizer behaves sanely.
+
+namespace y2020 {
+namespace control_loops {
+namespace drivetrain {
+namespace testing {
+
+using frc971::control_loops::drivetrain::DrivetrainConfig;
+using frc971::control_loops::drivetrain::Goal;
+using frc971::control_loops::drivetrain::LocalizerControl;
+using frc971::vision::sift::ImageMatchResult;
+using frc971::vision::sift::ImageMatchResultT;
+using frc971::vision::sift::CameraPoseT;
+using frc971::vision::sift::CameraCalibrationT;
+using frc971::vision::sift::TransformationMatrixT;
+
+namespace {
+DrivetrainConfig<double> GetTest2020DrivetrainConfig() {
+ DrivetrainConfig<double> config = GetDrivetrainConfig();
+ return config;
+}
+
+// Copies an Eigen matrix into a row-major vector of the data.
+std::vector<float> MatrixToVector(const Eigen::Matrix<double, 4, 4> &H) {
+ std::vector<float> data;
+ for (int row = 0; row < 4; ++row) {
+ for (int col = 0; col < 4; ++col) {
+ data.push_back(H(row, col));
+ }
+ }
+ return data;
+}
+
+// Provides the location of the turret to use for simulation. Mostly we care
+// about providing a location that is not perfectly aligned with the robot's
+// origin.
+Eigen::Matrix<double, 4, 4> TurretRobotTransformation() {
+ Eigen::Matrix<double, 4, 4> H;
+ H.setIdentity();
+ H.block<3, 1>(0, 3) << 1, 1.1, 0.9;
+ return H;
+}
+
+// Provides the location of the camera on the turret.
+// TODO(james): Also simulate a fixed camera that is *not* on the turret.
+Eigen::Matrix<double, 4, 4> CameraTurretTransformation() {
+ Eigen::Matrix<double, 4, 4> H;
+ H.setIdentity();
+ H.block<3, 1>(0, 3) << 0.1, 0, 0;
+ // Introduce a bit of pitch to make sure that we're exercising all the code.
+ H.block<3, 3>(0, 0) =
+ Eigen::AngleAxis<double>(0.1, Eigen::Vector3d::UnitY()) *
+ H.block<3, 3>(0, 0);
+ return H;
+}
+
+// The absolute target location to use. Not meant to correspond with a
+// particular field target.
+// TODO(james): Make more targets.
+std::vector<Eigen::Matrix<double, 4, 4>> TargetLocations() {
+ std::vector<Eigen::Matrix<double, 4, 4>> locations;
+ Eigen::Matrix<double, 4, 4> H;
+ H.setIdentity();
+ H.block<3, 1>(0, 3) << 10.0, 0, 0;
+ locations.push_back(H);
+ H.block<3, 1>(0, 3) << -10.0, 0, 0;
+ locations.push_back(H);
+ return locations;
+}
+} // namespace
+
+namespace chrono = std::chrono;
+using frc971::control_loops::drivetrain::testing::DrivetrainSimulation;
+using frc971::control_loops::drivetrain::DrivetrainLoop;
+using frc971::control_loops::drivetrain::testing::GetTestDrivetrainConfig;
+using aos::monotonic_clock;
+
+class LocalizedDrivetrainTest : public aos::testing::ControlLoopTest {
+ protected:
+ // We must use the 2020 drivetrain config so that we don't have to deal
+ // with shifting:
+ LocalizedDrivetrainTest()
+ : aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig(
+ "y2020/control_loops/drivetrain/simulation_config.json"),
+ GetTest2020DrivetrainConfig().dt),
+ test_event_loop_(MakeEventLoop("test")),
+ drivetrain_goal_sender_(
+ test_event_loop_->MakeSender<Goal>("/drivetrain")),
+ drivetrain_goal_fetcher_(
+ test_event_loop_->MakeFetcher<Goal>("/drivetrain")),
+ localizer_control_sender_(
+ test_event_loop_->MakeSender<LocalizerControl>("/drivetrain")),
+ superstructure_status_sender_(
+ test_event_loop_->MakeSender<superstructure::Status>(
+ "/superstructure")),
+ drivetrain_event_loop_(MakeEventLoop("drivetrain")),
+ dt_config_(GetTest2020DrivetrainConfig()),
+ camera_sender_(
+ test_event_loop_->MakeSender<ImageMatchResult>("/camera")),
+ localizer_(drivetrain_event_loop_.get(), dt_config_),
+ drivetrain_(dt_config_, drivetrain_event_loop_.get(), &localizer_),
+ drivetrain_plant_event_loop_(MakeEventLoop("plant")),
+ drivetrain_plant_(drivetrain_plant_event_loop_.get(), dt_config_),
+ last_frame_(monotonic_now()) {
+ set_team_id(frc971::control_loops::testing::kTeamNumber);
+ SetStartingPosition({3.0, 2.0, 0.0});
+ set_battery_voltage(12.0);
+
+ if (!FLAGS_output_file.empty()) {
+ log_buffer_writer_ = std::make_unique<aos::logger::DetachedBufferWriter>(
+ FLAGS_output_file);
+ logger_event_loop_ = MakeEventLoop("logger");
+ logger_ = std::make_unique<aos::logger::Logger>(log_buffer_writer_.get(),
+ logger_event_loop_.get());
+ }
+
+ test_event_loop_->MakeWatcher(
+ "/drivetrain",
+ [this](const frc971::control_loops::drivetrain::Status &) {
+ // Needs to do camera updates right after we run the control loop.
+ if (enable_cameras_) {
+ SendDelayedFrames();
+ if (last_frame_ + std::chrono::milliseconds(100) <
+ monotonic_now()) {
+ CaptureFrames();
+ last_frame_ = monotonic_now();
+ }
+ }
+ });
+
+ test_event_loop_->AddPhasedLoop(
+ [this](int) {
+ // Also use the opportunity to send out turret messages.
+ UpdateTurretPosition();
+ auto builder = superstructure_status_sender_.MakeBuilder();
+ auto turret_builder =
+ builder
+ .MakeBuilder<frc971::control_loops::
+ PotAndAbsoluteEncoderProfiledJointStatus>();
+ turret_builder.add_position(turret_position_);
+ turret_builder.add_velocity(turret_velocity_);
+ auto turret_offset = turret_builder.Finish();
+ auto status_builder = builder.MakeBuilder<superstructure::Status>();
+ status_builder.add_turret(turret_offset);
+ builder.Send(status_builder.Finish());
+ },
+ chrono::milliseconds(5));
+
+ // Run for enough time to allow the gyro/imu zeroing code to run.
+ RunFor(std::chrono::seconds(10));
+ }
+
+ virtual ~LocalizedDrivetrainTest() override {}
+
+ void SetStartingPosition(const Eigen::Matrix<double, 3, 1> &xytheta) {
+ *drivetrain_plant_.mutable_state() << xytheta.x(), xytheta.y(),
+ xytheta(2, 0), 0.0, 0.0;
+ Eigen::Matrix<double, Localizer::HybridEkf::kNStates, 1> localizer_state;
+ localizer_state.setZero();
+ localizer_state.block<3, 1>(0, 0) = xytheta;
+ localizer_.Reset(monotonic_now(), localizer_state);
+ }
+
+ void VerifyNearGoal(double eps = 1e-3) {
+ drivetrain_goal_fetcher_.Fetch();
+ EXPECT_NEAR(drivetrain_goal_fetcher_->left_goal(),
+ drivetrain_plant_.GetLeftPosition(), eps);
+ EXPECT_NEAR(drivetrain_goal_fetcher_->right_goal(),
+ drivetrain_plant_.GetRightPosition(), eps);
+ }
+
+ ::testing::AssertionResult IsNear(double expected, double actual,
+ double epsilon) {
+ if (std::abs(expected - actual) < epsilon) {
+ return ::testing::AssertionSuccess();
+ } else {
+ return ::testing::AssertionFailure()
+ << "Expected " << expected << " but got " << actual
+ << " with a max difference of " << epsilon
+ << " and an actual difference of " << std::abs(expected - actual);
+ }
+ }
+ ::testing::AssertionResult VerifyEstimatorAccurate(double eps) {
+ const Eigen::Matrix<double, 5, 1> true_state = drivetrain_plant_.state();
+ ::testing::AssertionResult result(true);
+ if (!(result = IsNear(localizer_.x(), true_state(0), eps))) {
+ return result;
+ }
+ if (!(result = IsNear(localizer_.y(), true_state(1), eps))) {
+ return result;
+ }
+ if (!(result = IsNear(localizer_.theta(), true_state(2), eps))) {
+ return result;
+ }
+ if (!(result = IsNear(localizer_.left_velocity(), true_state(3), eps))) {
+ return result;
+ }
+ if (!(result = IsNear(localizer_.right_velocity(), true_state(4), eps))) {
+ return result;
+ }
+ return result;
+ }
+
+ // Goes through and captures frames on the camera(s), queueing them up to be
+ // sent by SendDelayedFrames().
+ void CaptureFrames() {
+ const frc971::control_loops::Pose robot_pose(
+ {drivetrain_plant_.GetPosition().x(),
+ drivetrain_plant_.GetPosition().y(), 0.0},
+ drivetrain_plant_.state()(2, 0));
+ std::unique_ptr<ImageMatchResultT> frame(new ImageMatchResultT());
+
+ for (const auto &H_target_field : TargetLocations()) {
+ std::unique_ptr<CameraPoseT> camera_target(new CameraPoseT());
+
+ camera_target->field_to_target.reset(new TransformationMatrixT());
+ camera_target->field_to_target->data = MatrixToVector(H_target_field);
+
+ Eigen::Matrix<double, 4, 4> H_camera_turret =
+ Eigen::Matrix<double, 4, 4>::Identity();
+ if (is_turreted_) {
+ H_camera_turret = frc971::control_loops::TransformationMatrixForYaw(
+ turret_position_) *
+ CameraTurretTransformation();
+ }
+
+ // TODO(james): Use non-zero turret angles.
+ camera_target->camera_to_target.reset(new TransformationMatrixT());
+ camera_target->camera_to_target->data = MatrixToVector(
+ (robot_pose.AsTransformationMatrix() * TurretRobotTransformation() *
+ H_camera_turret).inverse() *
+ H_target_field);
+
+ frame->camera_poses.emplace_back(std::move(camera_target));
+ }
+
+ frame->image_monotonic_timestamp_ns =
+ chrono::duration_cast<chrono::nanoseconds>(
+ monotonic_now().time_since_epoch())
+ .count();
+ frame->camera_calibration.reset(new CameraCalibrationT());
+ {
+ frame->camera_calibration->fixed_extrinsics.reset(
+ new TransformationMatrixT());
+ TransformationMatrixT *H_turret_robot =
+ frame->camera_calibration->fixed_extrinsics.get();
+ H_turret_robot->data = MatrixToVector(TurretRobotTransformation());
+ }
+
+ if (is_turreted_) {
+ frame->camera_calibration->turret_extrinsics.reset(
+ new TransformationMatrixT());
+ TransformationMatrixT *H_camera_turret =
+ frame->camera_calibration->turret_extrinsics.get();
+ H_camera_turret->data = MatrixToVector(CameraTurretTransformation());
+ }
+
+ camera_delay_queue_.emplace(monotonic_now(), std::move(frame));
+ }
+
+ // Actually sends out all the camera frames.
+ void SendDelayedFrames() {
+ const std::chrono::milliseconds camera_latency(150);
+ while (!camera_delay_queue_.empty() &&
+ std::get<0>(camera_delay_queue_.front()) <
+ monotonic_now() - camera_latency) {
+ auto builder = camera_sender_.MakeBuilder();
+ ASSERT_TRUE(builder.Send(ImageMatchResult::Pack(
+ *builder.fbb(), std::get<1>(camera_delay_queue_.front()).get())));
+ camera_delay_queue_.pop();
+ }
+ }
+
+ std::unique_ptr<aos::EventLoop> test_event_loop_;
+ aos::Sender<Goal> drivetrain_goal_sender_;
+ aos::Fetcher<Goal> drivetrain_goal_fetcher_;
+ aos::Sender<LocalizerControl> localizer_control_sender_;
+ aos::Sender<superstructure::Status> superstructure_status_sender_;
+
+ std::unique_ptr<aos::EventLoop> drivetrain_event_loop_;
+ const frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ dt_config_;
+
+ aos::Sender<ImageMatchResult> camera_sender_;
+
+ Localizer localizer_;
+ DrivetrainLoop drivetrain_;
+
+ std::unique_ptr<aos::EventLoop> drivetrain_plant_event_loop_;
+ DrivetrainSimulation drivetrain_plant_;
+ monotonic_clock::time_point last_frame_;
+
+ // A queue of camera frames so that we can add a time delay to the data
+ // coming from the cameras.
+ std::queue<std::tuple<aos::monotonic_clock::time_point,
+ std::unique_ptr<ImageMatchResultT>>>
+ camera_delay_queue_;
+
+ void set_enable_cameras(bool enable) { enable_cameras_ = enable; }
+ void set_camera_is_turreted(bool turreted) { is_turreted_ = turreted; }
+
+ void set_turret(double position, double velocity) {
+ turret_position_ = position;
+ turret_velocity_ = velocity;
+ }
+
+ void SendGoal(double left, double right) {
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
+ drivetrain_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType::MOTION_PROFILE);
+ drivetrain_builder.add_left_goal(left);
+ drivetrain_builder.add_right_goal(right);
+
+ EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
+ }
+
+ private:
+ void UpdateTurretPosition() {
+ turret_position_ +=
+ turret_velocity_ *
+ aos::time::DurationInSeconds(monotonic_now() - last_turret_update_);
+ last_turret_update_ = monotonic_now();
+ }
+
+ bool enable_cameras_ = false;
+ // Whether to make the camera be on the turret or not.
+ bool is_turreted_ = true;
+
+ // The time at which we last incremented turret_position_.
+ monotonic_clock::time_point last_turret_update_ = monotonic_clock::min_time;
+ // Current turret position and velocity. These are set directly by the user in
+ // the test, and if velocity is non-zero, then we will automatically increment
+ // turret_position_ with every timestep.
+ double turret_position_ = 0.0; // rad
+ double turret_velocity_ = 0.0; // rad / sec
+
+ std::unique_ptr<aos::EventLoop> logger_event_loop_;
+ std::unique_ptr<aos::logger::DetachedBufferWriter> log_buffer_writer_;
+ std::unique_ptr<aos::logger::Logger> logger_;
+};
+
+// Tests that no camera updates, combined with a perfect model, results in no
+// error.
+TEST_F(LocalizedDrivetrainTest, NoCameraUpdate) {
+ SetEnabled(true);
+ set_enable_cameras(false);
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-7));
+
+ SendGoal(-1.0, 1.0);
+
+ RunFor(chrono::seconds(3));
+ VerifyNearGoal();
+ EXPECT_TRUE(VerifyEstimatorAccurate(5e-4));
+}
+
+// Tests that camera updates with a perfect models results in no errors.
+TEST_F(LocalizedDrivetrainTest, PerfectCameraUpdate) {
+ SetEnabled(true);
+ set_enable_cameras(true);
+ set_camera_is_turreted(true);
+
+ SendGoal(-1.0, 1.0);
+
+ RunFor(chrono::seconds(3));
+ VerifyNearGoal();
+ EXPECT_TRUE(VerifyEstimatorAccurate(5e-4));
+}
+
+// Tests that camera updates with a constant initial error in the position
+// results in convergence.
+TEST_F(LocalizedDrivetrainTest, InitialPositionError) {
+ SetEnabled(true);
+ set_enable_cameras(true);
+ set_camera_is_turreted(true);
+ drivetrain_plant_.mutable_state()->topRows(3) +=
+ Eigen::Vector3d(0.1, 0.1, 0.01);
+
+ // Confirm that some translational movement does get handled correctly.
+ SendGoal(-0.9, 1.0);
+
+ // Give the filters enough time to converge.
+ RunFor(chrono::seconds(10));
+ VerifyNearGoal(5e-3);
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
+}
+
+// Tests that camera updates using a non-turreted camera work.
+TEST_F(LocalizedDrivetrainTest, InitialPositionErrorNoTurret) {
+ SetEnabled(true);
+ set_enable_cameras(true);
+ set_camera_is_turreted(false);
+ drivetrain_plant_.mutable_state()->topRows(3) +=
+ Eigen::Vector3d(0.1, 0.1, 0.01);
+
+ SendGoal(-1.0, 1.0);
+
+ // Give the filters enough time to converge.
+ RunFor(chrono::seconds(10));
+ VerifyNearGoal(5e-3);
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
+}
+
+// Tests that we are able to handle a constant, non-zero turret angle.
+TEST_F(LocalizedDrivetrainTest, NonZeroTurret) {
+ SetEnabled(true);
+ set_enable_cameras(true);
+ set_camera_is_turreted(true);
+ set_turret(1.0, 0.0);
+ drivetrain_plant_.mutable_state()->topRows(3) +=
+ Eigen::Vector3d(0.1, 0.1, 0.0);
+
+ SendGoal(-1.0, 1.0);
+
+ // Give the filters enough time to converge.
+ RunFor(chrono::seconds(10));
+ VerifyNearGoal(5e-3);
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-3));
+}
+
+// Tests that we are able to handle a constant velocity turret.
+TEST_F(LocalizedDrivetrainTest, MovingTurret) {
+ SetEnabled(true);
+ set_enable_cameras(true);
+ set_camera_is_turreted(true);
+ set_turret(0.0, 0.2);
+ drivetrain_plant_.mutable_state()->topRows(3) +=
+ Eigen::Vector3d(0.1, 0.1, 0.0);
+
+ SendGoal(-1.0, 1.0);
+
+ // Give the filters enough time to converge.
+ RunFor(chrono::seconds(10));
+ VerifyNearGoal(5e-3);
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-3));
+}
+
+// Tests that we reject camera measurements when the turret is spinning too
+// fast.
+TEST_F(LocalizedDrivetrainTest, TooFastTurret) {
+ SetEnabled(true);
+ set_enable_cameras(true);
+ set_camera_is_turreted(true);
+ set_turret(0.0, -10.0);
+ const Eigen::Vector3d disturbance(0.1, 0.1, 0.0);
+ drivetrain_plant_.mutable_state()->topRows(3) += disturbance;
+
+ SendGoal(-1.0, 1.0);
+
+ RunFor(chrono::seconds(10));
+ EXPECT_FALSE(VerifyEstimatorAccurate(1e-3));
+ // If we remove the disturbance, we should now be correct.
+ drivetrain_plant_.mutable_state()->topRows(3) -= disturbance;
+ VerifyNearGoal(5e-3);
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-3));
+}
+
+// Tests that we don't reject camera measurements when the turret is spinning
+// too fast but we aren't using a camera attached to the turret.
+TEST_F(LocalizedDrivetrainTest, TooFastTurretDoesntAffectFixedCamera) {
+ SetEnabled(true);
+ set_enable_cameras(true);
+ set_camera_is_turreted(false);
+ set_turret(0.0, -10.0);
+ const Eigen::Vector3d disturbance(0.1, 0.1, 0.0);
+ drivetrain_plant_.mutable_state()->topRows(3) += disturbance;
+
+ SendGoal(-1.0, 1.0);
+
+ RunFor(chrono::seconds(10));
+ VerifyNearGoal(5e-3);
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-3));
+}
+
+} // namespace testing
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace y2020
diff --git a/y2020/vision/sift/BUILD b/y2020/vision/sift/BUILD
index 5cfb6aa..02507a9 100644
--- a/y2020/vision/sift/BUILD
+++ b/y2020/vision/sift/BUILD
@@ -1,5 +1,5 @@
load(":fast_gaussian.bzl", "fast_gaussian")
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_py_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_py_library", "flatbuffer_ts_library")
cc_binary(
name = "fast_gaussian_generator",
@@ -204,6 +204,7 @@
"TrainingImage",
"TrainingData",
],
+ visibility = ["//visibility:public"],
)
flatbuffer_cc_library(
@@ -213,6 +214,12 @@
visibility = ["//visibility:public"],
)
+flatbuffer_ts_library(
+ name = "sift_ts_fbs",
+ srcs = ["sift.fbs"],
+ visibility = ["//y2020:__subpackages__"],
+)
+
flatbuffer_cc_library(
name = "sift_training_fbs",
srcs = ["sift_training.fbs"],
diff --git a/y2020/vision/tools/python_code/BUILD b/y2020/vision/tools/python_code/BUILD
new file mode 100644
index 0000000..670b664
--- /dev/null
+++ b/y2020/vision/tools/python_code/BUILD
@@ -0,0 +1,49 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_py_library")
+
+py_binary(
+ name = "load_sift_training",
+ data = [
+ ":test_images/train_power_port_red.png",
+ ":test_images/train_power_port_red_webcam.png",
+ ":test_images/train_power_port_blue.png",
+ ":test_images/train_loading_bay_red.png",
+ ":test_images/train_loading_bay_blue.png",
+ ],
+ srcs = ["load_sift_training.py",
+ "camera_definition.py",
+ "define_training_data.py",
+ "target_definition.py",
+ "train_and_match.py",
+ ],
+ args = ["sift_training_data.h",
+ ],
+ default_python_version = "PY3",
+ srcs_version = "PY2AND3",
+ deps = [
+ "//y2020/vision/sift:sift_fbs_python",
+ "@opencv_contrib_nonfree_amd64//:python_opencv",
+ "@bazel_tools//tools/python/runfiles",
+ ],
+)
+
+genrule(
+ name = "run_load_sift_training",
+ outs = [
+ "sift_training_data.h",
+ ],
+ cmd = " ".join([
+ "$(location :load_sift_training)",
+ "$(location sift_training_data.h)",
+ ]),
+ tools = [
+ ":load_sift_training",
+ ],
+)
+
+cc_library(
+ name = "sift_training",
+ hdrs = [
+ "sift_training_data.h",
+ ],
+ visibility = ["//visibility:public"],
+)
diff --git a/y2020/vision/tools/python_code/camera_definition.py b/y2020/vision/tools/python_code/camera_definition.py
new file mode 100644
index 0000000..59f008f
--- /dev/null
+++ b/y2020/vision/tools/python_code/camera_definition.py
@@ -0,0 +1,57 @@
+import argparse
+import cv2
+import json
+import math
+import numpy as np
+import time
+
+
+class CameraIntrinsics:
+ def __init__(self):
+ self.camera_matrix = []
+ self.distortion_coeffs = []
+
+ pass
+
+class CameraExtrinsics:
+ def __init__(self):
+ self.R = []
+ self.T = []
+
+ pass
+
+class CameraParameters:
+ def __init__(self):
+ self.camera_int = CameraIntrinsics()
+ self.camera_ext = CameraExtrinsics()
+
+ pass
+
+
+### CAMERA DEFINITIONS
+
+# Robot camera has:
+# FOV_H = 93.*math.pi()/180.
+# FOV_V = 70.*math.pi()/180.
+
+# Create fake camera (based on USB webcam params)
+fx = 810.
+fy = 810.
+cx = 320.
+cy = 240.
+
+# Define a web_cam
+web_cam_int = CameraIntrinsics()
+web_cam_int.camera_matrix = np.asarray([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
+web_cam_int.distortion_coeffs = np.zeros((5,1))
+
+web_cam_ext = CameraExtrinsics()
+# Camera rotation from robot x,y,z to opencv (z, -x, -y)
+web_cam_ext.R = np.array([[0., 0., 1.],
+ [-1, 0, 0],
+ [0, -1., 0]])
+web_cam_ext.T = np.array([[0., 0., 0.]]).T
+
+web_cam_params = CameraParameters()
+web_cam_params.camera_int = web_cam_int
+web_cam_params.camera_ext = web_cam_ext
diff --git a/y2020/vision/tools/python_code/define_training_data.py b/y2020/vision/tools/python_code/define_training_data.py
new file mode 100644
index 0000000..22eb6ce
--- /dev/null
+++ b/y2020/vision/tools/python_code/define_training_data.py
@@ -0,0 +1,255 @@
+import argparse
+import cv2
+import json
+import math
+import numpy as np
+import time
+
+import train_and_match as tam
+
+# Points for current polygon
+point_list = []
+current_mouse = (0, 0)
+
+
+def get_mouse_event(event, x, y, flags, param):
+ global point_list
+ global current_mouse
+ current_mouse = (x, y)
+ if event == cv2.EVENT_LBUTTONUP:
+ #print("Adding point at %d, %d" % (x,y))
+ point_list.append([x, y])
+ pass
+
+
+def draw_polygon(image, polygon, color=(255, 0, 0), close_polygon=False):
+ for point in polygon:
+ image = cv2.circle(image, (point[0], point[1]), 5, (255, 0, 0), -1)
+ if (len(polygon) > 1):
+ np_poly = np.array(polygon)
+ image = cv2.polylines(
+ image, [np_poly], close_polygon, color, thickness=3)
+ return image
+
+
+# Close out polygon, return True if size is 3 or more points
+def finish_polygon(image, polygon):
+ global point_list
+ # If we have at least 3 points, close and show the polygon
+ if len(point_list) <= 2:
+ return False
+
+ point_list.append(point_list[0])
+ image = draw_polygon(image, point_list, color=(0, 0, 255))
+ cv2.imshow("image", image)
+ cv2.waitKey(500)
+ return True
+
+
+def define_polygon(image):
+ global point_list
+ # Set of all defined polygons
+ point_list = []
+ polygon_list = []
+
+ display_image = image.copy()
+
+ cv2.namedWindow("image")
+ cv2.setMouseCallback("image", get_mouse_event)
+
+ while True:
+ cv2.imshow("image", display_image)
+ key = cv2.waitKey(1) & 0xFF
+
+ # if the 'r' key is pressed, reset the current polygon
+ # Leaves previously defined polygons intact
+ if key == ord("r"):
+ display_image = image.copy()
+ point_list = []
+
+ # if the 'd' key is pressed, delete the last point
+ if key == ord("d"):
+ display_image = image.copy()
+ point_list.pop()
+
+ # if the 'n' key is pressed, save current polygon, and move to next
+ if key == ord("n"):
+ if (finish_polygon(display_image, point_list)):
+ polygon_list.append(point_list)
+ display_image = image.copy()
+ point_list = []
+
+ # if the 'q' key is pressed, break from the loop and finish polygon
+ elif key == ord("q"):
+ if (finish_polygon(display_image, point_list)):
+ polygon_list.append(point_list)
+ break
+
+ display_image = draw_polygon(display_image, point_list)
+
+ return polygon_list
+
+
+# Given a list of points on an image, prompt the user to click on the
+# corresponding points within the image.
+# Return the list of those points that have been clicked
+def define_points_by_list(image, points):
+ global point_list
+ global current_mouse
+ point_list = []
+
+ display_image = image.copy()
+
+ cv2.namedWindow("image")
+ cv2.setMouseCallback("image", get_mouse_event)
+
+ while (len(point_list) < len(points)):
+ i = len(point_list)
+ # Draw mouse location and suggested target
+ display_image = image.copy()
+ display_image = cv2.circle(display_image, (points[i][0], points[i][1]),
+ 15, (0, 255, 0), 2)
+ cursor_length = 5
+ display_image = cv2.line(
+ display_image,
+ (current_mouse[0] - cursor_length, current_mouse[1]),
+ (current_mouse[0] + cursor_length, current_mouse[1]), (255, 0, 0),
+ 2, cv2.LINE_AA)
+ display_image = cv2.line(
+ display_image,
+ (current_mouse[0], current_mouse[1] - cursor_length),
+ (current_mouse[0], current_mouse[1] + cursor_length), (255, 0, 0),
+ 2, cv2.LINE_AA)
+
+ cv2.imshow("image", display_image)
+
+ key = cv2.waitKey(1) & 0xFF
+
+ # if the 'r' key is pressed, reset the current point collection
+ # Leaves previously defined polygons intact
+ if key == ord("r"):
+ draw_image = image.copy()
+ point_list = []
+
+ # if the 'd' key is pressed, delete the last point
+ elif key == ord("d"):
+ draw_image = image.copy()
+ point_list.pop()
+
+ # if the 'q' key is pressed, quit
+ elif key == ord("q"):
+ quit()
+
+ return point_list
+
+# Determine whether a given point lies within (or on border of) a set of polygons
+# Return true if it does
+def point_in_polygons(point, polygons):
+ for poly in polygons:
+ np_poly = np.asarray(poly)
+ dist = cv2.pointPolygonTest(np_poly, (point[0], point[1]), True)
+ if dist >=0:
+ return True
+
+ return False
+
+## Filter keypoints by polygons
+def filter_keypoints_by_polygons(keypoint_list, descriptor_list, polygons):
+ # TODO: Need to make sure we've got the right numpy array / list
+ keep_keypoint_list = []
+ keep_descriptor_list = []
+ reject_keypoint_list = []
+ reject_descriptor_list = []
+ keep_list = []
+ reject_list = []
+
+ # For now, pretend keypoints are just points
+ for i in range(len(keypoint_list)):
+ if point_in_polygons((keypoint_list[i].pt[0], keypoint_list[i].pt[1]), polygons):
+ keep_list.append(i)
+ else:
+ reject_list.append(i)
+
+ keep_keypoint_list = [keypoint_list[kp_ind] for kp_ind in keep_list]
+ reject_keypoint_list = [keypoint_list[kp_ind] for kp_ind in reject_list]
+ # Allow function to be called with no descriptors, and just return empty list
+ if descriptor_list is not None:
+ keep_descriptor_list = descriptor_list[keep_list,:]
+ reject_descriptor_list = descriptor_list[reject_list,:]
+
+ return keep_keypoint_list, keep_descriptor_list, reject_keypoint_list, reject_descriptor_list, keep_list
+
+# Helper function that appends a column of ones to a list (of 2d points)
+def append_ones(point_list):
+ return np.hstack([point_list, np.ones((len(point_list),1))])
+
+# Given a list of 2d points and thei corresponding 3d locations, compute map
+# between them.
+# pt_3d = (pt_2d, 1) * reprojection_map
+# TODO: We should really look at residuals to see if things are messed up
+def compute_reprojection_map(polygon_2d, polygon_3d):
+ pts_2d = np.asarray(np.float32(polygon_2d)).reshape(-1,2)
+ pts_2d_lstsq = append_ones(pts_2d)
+ pts_3d_lstsq = np.asarray(np.float32(polygon_3d)).reshape(-1,3)
+
+ reprojection_map = np.linalg.lstsq(pts_2d_lstsq, pts_3d_lstsq, rcond=-1)[0]
+
+ return reprojection_map
+
+# Given set of keypoints (w/ 2d location), a reprojection map, and a polygon
+# that defines regions for where this is valid
+# Returns a numpy array of 3d locations the same size as the keypoint list
+def compute_3d_points(keypoint_2d_list, reprojection_map):
+ pt_2d_lstsq = append_ones(np.asarray(np.float32(keypoint_2d_list)).reshape(-1,2))
+ pt_3d = pt_2d_lstsq.dot(reprojection_map)
+
+ return pt_3d
+
+# Given 2d and 3d locations, and camera location and projection model,
+# display locations on an image
+def visualize_reprojections(img, pts_2d, pts_3d, cam_mat, distortion_coeffs):
+ # Compute camera location
+ # TODO: Warn on bad inliers
+ # TODO: Change this to not have to recast to np
+ pts_2d_np = np.asarray(np.float32(pts_2d)).reshape(-1, 1, 2)
+ pts_3d_np = np.asarray(np.float32(pts_3d)).reshape(-1, 1, 3)
+ retval, R, T, inliers = cv2.solvePnPRansac(pts_3d_np, pts_2d_np, cam_mat,
+ distortion_coeffs)
+ pts_3d_proj_2d, jac_2d = cv2.projectPoints(pts_3d_np, R, T, cam_mat,
+ distortion_coeffs)
+ if inliers is None:
+ print("WARNING: Didn't get any inliers when reprojecting polygons")
+ return img
+ for i in range(len(pts_2d)):
+ pt_2d = pts_2d_np[i][0]
+ pt_3d_proj = pts_3d_proj_2d[i][0]
+ pt_color = (0, 255, 0)
+ if i not in inliers:
+ pt_color = (0, 0, 255)
+
+ img = cv2.circle(img, (pt_2d[0], pt_2d[1]), 3, pt_color, 3)
+ img = cv2.circle(img, (pt_3d_proj[0], pt_3d_proj[1]), 15, pt_color, 3)
+
+ cv2.imshow("image", img)
+ cv2.waitKey(0)
+ return img
+
+
+def sample_define_polygon_usage():
+ image = cv2.imread("test_images/train_power_port_red.png")
+
+ polygon_list = define_polygon(image)
+ print(polygon_list)
+
+
+def sample_define_points_by_list_usage():
+ image = cv2.imread("test_images/train_power_port_red.png")
+
+ test_points = [(451, 679), (451, 304),
+ (100, 302), (451, 74),
+ (689, 74), (689, 302),
+ (689, 679)]
+
+ polygon_list = define_points_by_list(image, test_points)
+ print(polygon_list)
+
diff --git a/y2020/vision/tools/python_code/field_display.py b/y2020/vision/tools/python_code/field_display.py
index a3f1024..93f645c 100644
--- a/y2020/vision/tools/python_code/field_display.py
+++ b/y2020/vision/tools/python_code/field_display.py
@@ -1,6 +1,5 @@
import cv2
import math
-from matplotlib import pyplot as plt
import numpy as np
diff --git a/y2020/vision/tools/python_code/image_match_test.py b/y2020/vision/tools/python_code/image_match_test.py
index bcf610f..d6b7deb 100644
--- a/y2020/vision/tools/python_code/image_match_test.py
+++ b/y2020/vision/tools/python_code/image_match_test.py
@@ -100,7 +100,7 @@
looping = True
# Grab images until 'q' is pressed (or just once for canned images)
-while (looping):
+while looping:
if (query_image_index is -1):
# Capture frame-by-frame
ret, frame = cap.read()
@@ -305,3 +305,4 @@
cap.release()
cv2.destroyAllWindows()
+
diff --git a/y2020/vision/tools/python_code/load_sift_training.py b/y2020/vision/tools/python_code/load_sift_training.py
new file mode 100644
index 0000000..9fa4acf
--- /dev/null
+++ b/y2020/vision/tools/python_code/load_sift_training.py
@@ -0,0 +1,100 @@
+#!/usr/bin/python3
+
+import cv2
+import sys
+import flatbuffers
+import target_definition
+
+import frc971.vision.sift.TrainingImage as TrainingImage
+import frc971.vision.sift.TrainingData as TrainingData
+import frc971.vision.sift.Feature as Feature
+
+def main():
+
+ output_path = sys.argv[1]
+ print("Writing file to ", output_path)
+
+ target_data_list = target_definition.compute_target_definition()
+
+ fbb = flatbuffers.Builder(0)
+
+ images_vector = []
+
+ for target_data in target_data_list:
+
+ features_vector = []
+
+ for keypoint, keypoint_3d, descriptor in zip(target_data.keypoint_list,
+ target_data.keypoint_list_3d,
+ target_data.descriptor_list):
+
+ Feature.FeatureStartDescriptorVector(fbb, len(descriptor))
+ for n in reversed(descriptor):
+ fbb.PrependFloat32(n)
+ descriptor_vector = fbb.EndVector(len(descriptor))
+
+ Feature.FeatureStart(fbb)
+
+ Feature.FeatureAddDescriptor(fbb, descriptor_vector)
+ Feature.FeatureAddX(fbb, keypoint.pt[0])
+ Feature.FeatureAddY(fbb, keypoint.pt[1])
+ Feature.FeatureAddSize(fbb, keypoint.size)
+ Feature.FeatureAddAngle(fbb, keypoint.angle)
+ Feature.FeatureAddResponse(fbb, keypoint.response)
+ Feature.FeatureAddOctave(fbb, keypoint.octave)
+
+ features_vector.append(Feature.FeatureEnd(fbb))
+
+ ## TODO: Write 3d vector here
+
+ TrainingImage.TrainingImageStartFeaturesVector(fbb, len(features_vector))
+ for feature in reversed(features_vector):
+ fbb.PrependUOffsetTRelative(feature)
+ features_vector_table = fbb.EndVector(len(features_vector))
+
+ TrainingImage.TrainingImageStart(fbb)
+ TrainingImage.TrainingImageAddFeatures(fbb, features_vector_table)
+ # TODO(Brian): Fill out the transformation matrices.
+ images_vector.append(TrainingImage.TrainingImageEnd(fbb))
+
+ TrainingData.TrainingDataStartImagesVector(fbb, len(images_vector))
+ for training_image in reversed(images_vector):
+ fbb.PrependUOffsetTRelative(training_image)
+ images_vector_table = fbb.EndVector(len(images_vector))
+
+ TrainingData.TrainingDataStart(fbb)
+ TrainingData.TrainingDataAddImages(fbb, images_vector_table)
+ fbb.Finish(TrainingData.TrainingDataEnd(fbb))
+
+ bfbs = fbb.Output()
+
+ output_prefix = [
+ b'#ifndef Y2020_VISION_TOOLS_PYTHON_CODE_TRAINING_DATA_H_',
+ b'#define Y2020_VISION_TOOLS_PYTHON_CODE_TRAINING_DATA_H_',
+ b'#include <string_view>',
+ b'namespace frc971 {',
+ b'namespace vision {',
+ b'inline std::string_view SiftTrainingData() {',
+ ]
+ output_suffix = [
+ b' return std::string_view(kData, sizeof(kData));',
+ b'}',
+ b'} // namespace vision',
+ b'} // namespace frc971',
+ b'#endif // Y2020_VISION_TOOLS_PYTHON_CODE_TRAINING_DATA_H_',
+ ]
+
+ with open(output_path, 'wb') as output:
+ for line in output_prefix:
+ output.write(line)
+ output.write(b'\n')
+ output.write(b'alignas(64) static constexpr char kData[] = "')
+ for byte in fbb.Output():
+ output.write(b'\\x' + (b'%x' % byte).zfill(2))
+ output.write(b'";\n')
+ for line in output_suffix:
+ output.write(line)
+ output.write(b'\n')
+
+if __name__ == '__main__':
+ main()
diff --git a/y2020/vision/tools/python_code/target_definition.py b/y2020/vision/tools/python_code/target_definition.py
new file mode 100644
index 0000000..779eb0b
--- /dev/null
+++ b/y2020/vision/tools/python_code/target_definition.py
@@ -0,0 +1,414 @@
+import argparse
+import cv2
+import json
+import math
+import numpy as np
+
+import camera_definition
+import define_training_data as dtd
+import train_and_match as tam
+
+global VISUALIZE_KEYPOINTS
+global USE_BAZEL
+USE_BAZEL = True
+VISUALIZE_KEYPOINTS = False
+
+def bazel_name_fix(filename):
+ ret_name = filename
+ if USE_BAZEL:
+ ret_name = 'org_frc971/y2020/vision/tools/python_code/' + filename
+
+ return ret_name
+
+class TargetData:
+ def __init__(self, filename):
+ self.image_filename = filename
+ # Load an image (will come in as a 1-element list)
+ if USE_BAZEL:
+ from bazel_tools.tools.python.runfiles import runfiles
+ r = runfiles.Create()
+ self.image_filename = r.Rlocation(bazel_name_fix(self.image_filename))
+ self.image = tam.load_images([self.image_filename])[0]
+ self.polygon_list = []
+ self.polygon_list_3d = []
+ self.reprojection_map_list = []
+ self.keypoint_list = []
+ self.keypoint_list_3d = None # numpy array of 3D points
+ self.descriptor_list = []
+
+ def extract_features(self, feature_extractor):
+ kp_lists, desc_lists = tam.detect_and_compute(feature_extractor,
+ [self.image])
+ self.keypoint_list = kp_lists[0]
+ self.descriptor_list = desc_lists[0]
+
+ def filter_keypoints_by_polygons(self):
+ self.keypoint_list, self.descriptor_list, _, _, _ = dtd.filter_keypoints_by_polygons(
+ self.keypoint_list, self.descriptor_list, self.polygon_list)
+
+ def compute_reprojection_maps(self):
+ for poly_ind in range(len(self.polygon_list)):
+ reprojection_map = dtd.compute_reprojection_map(
+ self.polygon_list[poly_ind], self.polygon_list_3d[poly_ind])
+ self.reprojection_map_list.append(reprojection_map)
+
+ def project_keypoint_to_3d_by_polygon(self, keypoint_list):
+ # Create dummy array of correct size that we can put values in
+ point_list_3d = np.asarray(
+ [(0., 0., 0.) for kp in keypoint_list]).reshape(-1, 3)
+ # Iterate through our polygons
+ for poly_ind in range(len(self.polygon_list)):
+ # Filter and project points for each polygon in the list
+ filtered_keypoints, _, _, _, keep_list = dtd.filter_keypoints_by_polygons(
+ keypoint_list, None, [self.polygon_list[poly_ind]])
+ print("Filtering kept %d of %d features" % (len(keep_list),
+ len(keypoint_list)))
+ filtered_point_array = np.asarray(
+ [(keypoint.pt[0], keypoint.pt[1])
+ for keypoint in filtered_keypoints]).reshape(-1, 2)
+ filtered_point_list_3d = dtd.compute_3d_points(
+ filtered_point_array, self.reprojection_map_list[poly_ind])
+ for i in range(len(keep_list)):
+ point_list_3d[keep_list[i]] = filtered_point_list_3d[i]
+
+ return point_list_3d
+
+def compute_target_definition():
+ ############################################################
+ # TARGET DEFINITIONS
+ ############################################################
+
+ ideal_target_list = []
+ training_target_list = []
+
+ # Some general info about our field and targets
+ # Assume camera centered on target at 1 m above ground and distance of 4.85m
+ field_length = 15.98
+ power_port_total_height = 3.10
+ power_port_center_y = 1.67
+ power_port_width = 1.22
+ power_port_bottom_wing_height = 1.88
+ power_port_wing_width = 1.83
+ loading_bay_edge_y = 1.11
+ loading_bay_width = 1.52
+ loading_bay_height = 0.94
+
+ # Pick the target center location at halfway between top and bottom of the top panel
+ target_center_height = (power_port_total_height + power_port_bottom_wing_height) / 2.
+
+ # TODO: Still need to figure out what this angle actually is
+ wing_angle = 20. * math.pi / 180.
+
+ ###
+ ### Red Power Port
+ ###
+
+ # Create the reference "ideal" image
+ ideal_power_port_red = TargetData('test_images/train_power_port_red.png')
+
+ # Start at lower left corner, and work around clockwise
+ # These are taken by manually finding the points in gimp for this image
+ power_port_red_main_panel_polygon_points_2d = [(451, 679), (451, 304),
+ (100, 302), (451, 74),
+ (689, 74), (689, 302),
+ (689, 679)]
+
+ # These are "virtual" 3D points based on the expected geometry
+ power_port_red_main_panel_polygon_points_3d = [
+ (field_length, -power_port_center_y + power_port_width / 2., 0.),
+ (field_length, -power_port_center_y + power_port_width / 2.,
+ power_port_bottom_wing_height),
+ (field_length, -power_port_center_y + power_port_width / 2.
+ + power_port_wing_width, power_port_bottom_wing_height),
+ (field_length, -power_port_center_y + power_port_width / 2.,
+ power_port_total_height),
+ (field_length, -power_port_center_y - power_port_width / 2.,
+ power_port_total_height),
+ (field_length, -power_port_center_y - power_port_width / 2.,
+ power_port_bottom_wing_height),
+ (field_length, -power_port_center_y - power_port_width / 2., 0.)
+ ]
+
+ power_port_red_wing_panel_polygon_points_2d = [(689, 74), (1022, 302),
+ (689, 302)]
+ # These are "virtual" 3D points based on the expected geometry
+ power_port_red_wing_panel_polygon_points_3d = [
+ (field_length, -power_port_center_y - power_port_width / 2.,
+ power_port_total_height),
+ (field_length - power_port_wing_width * math.sin(wing_angle),
+ -power_port_center_y - power_port_width / 2.
+ - power_port_wing_width * math.cos(wing_angle),
+ power_port_bottom_wing_height),
+ (field_length, -power_port_center_y - power_port_width / 2.,
+ power_port_bottom_wing_height)
+ ]
+
+ # Populate the red power port
+ ideal_power_port_red.polygon_list.append(power_port_red_main_panel_polygon_points_2d)
+ ideal_power_port_red.polygon_list_3d.append(power_port_red_main_panel_polygon_points_3d)
+
+ ideal_power_port_red.polygon_list.append(power_port_red_wing_panel_polygon_points_2d)
+ ideal_power_port_red.polygon_list_3d.append(power_port_red_wing_panel_polygon_points_3d)
+
+ # Add the ideal 3D target to our list
+ ideal_target_list.append(ideal_power_port_red)
+ # And add the training image we'll actually use to the training list
+ training_target_list.append(TargetData('test_images/train_power_port_red_webcam.png'))
+
+ ###
+ ### Red Loading Bay
+ ###
+
+ ideal_loading_bay_red = TargetData('test_images/train_loading_bay_red.png')
+
+ # Start at lower left corner, and work around clockwise
+ # These are taken by manually finding the points in gimp for this image
+ loading_bay_red_polygon_points_2d = [(42, 406), (42, 35), (651, 34), (651, 406)]
+
+ # These are "virtual" 3D points based on the expected geometry
+ loading_bay_red_polygon_points_3d = [
+ (field_length, loading_bay_edge_y + loading_bay_width, 0.),
+ (field_length, loading_bay_edge_y + loading_bay_width, loading_bay_height),
+ (field_length, loading_bay_edge_y, loading_bay_height),
+ (field_length, loading_bay_edge_y, 0.)
+ ]
+
+ ideal_loading_bay_red.polygon_list.append(loading_bay_red_polygon_points_2d)
+ ideal_loading_bay_red.polygon_list_3d.append(loading_bay_red_polygon_points_3d)
+
+ ideal_target_list.append(ideal_loading_bay_red)
+ training_target_list.append(TargetData('test_images/train_loading_bay_red.png'))
+
+ ###
+ ### Blue Power Port
+ ###
+
+ ideal_power_port_blue = TargetData('test_images/train_power_port_blue.png')
+
+ # Start at lower left corner, and work around clockwise
+ # These are taken by manually finding the points in gimp for this image
+ power_port_blue_main_panel_polygon_points_2d = [(438, 693), (438, 285),
+ (93, 285), (440, 50),
+ (692, 50), (692, 285),
+ (692, 693)]
+
+ # These are "virtual" 3D points based on the expected geometry
+ power_port_blue_main_panel_polygon_points_3d = [
+ (0., power_port_center_y - power_port_width / 2., 0.),
+ (0., power_port_center_y - power_port_width / 2.,
+ power_port_bottom_wing_height),
+ (0., power_port_center_y - power_port_width / 2. - power_port_wing_width,
+ power_port_bottom_wing_height),
+ (0., power_port_center_y - power_port_width / 2.,
+ power_port_total_height),
+ (0., power_port_center_y + power_port_width / 2.,
+ power_port_total_height),
+ (0., power_port_center_y + power_port_width / 2.,
+ power_port_bottom_wing_height),
+ (0., power_port_center_y + power_port_width / 2., 0.)
+ ]
+
+ power_port_blue_wing_panel_polygon_points_2d = [(692, 50), (1047, 285),
+ (692, 285)]
+ # These are "virtual" 3D points based on the expected geometry
+ power_port_blue_wing_panel_polygon_points_3d = [
+ (0., power_port_center_y + power_port_width / 2.,
+ power_port_total_height),
+ (power_port_wing_width * math.sin(wing_angle),
+ power_port_center_y - power_port_width / 2. +
+ power_port_wing_width * math.cos(wing_angle),
+ power_port_bottom_wing_height),
+ (0., power_port_center_y + power_port_width / 2.,
+ power_port_bottom_wing_height)
+ ]
+
+ # Populate the blue power port
+ ideal_power_port_blue.polygon_list.append(power_port_blue_main_panel_polygon_points_2d)
+ ideal_power_port_blue.polygon_list_3d.append(power_port_blue_main_panel_polygon_points_3d)
+
+ ideal_power_port_blue.polygon_list.append(power_port_blue_wing_panel_polygon_points_2d)
+ ideal_power_port_blue.polygon_list_3d.append(power_port_blue_wing_panel_polygon_points_3d)
+
+ ideal_target_list.append(ideal_power_port_blue)
+ training_target_list.append(TargetData('test_images/train_power_port_blue.png'))
+
+ ###
+ ### Blue Loading Bay
+ ###
+
+ ideal_loading_bay_blue = TargetData('test_images/train_loading_bay_blue.png')
+
+ # Start at lower left corner, and work around clockwise
+ # These are taken by manually finding the points in gimp for this image
+ loading_bay_blue_polygon_points_2d = [(7, 434), (7, 1), (729, 1), (729, 434)]
+
+ # These are "virtual" 3D points based on the expected geometry
+ loading_bay_blue_polygon_points_3d = [
+ (field_length, loading_bay_edge_y + loading_bay_width, 0.),
+ (field_length, loading_bay_edge_y + loading_bay_width, loading_bay_height),
+ (field_length, loading_bay_edge_y, loading_bay_height),
+ (field_length, loading_bay_edge_y, 0.)
+ ]
+
+ ideal_loading_bay_blue.polygon_list.append(loading_bay_blue_polygon_points_2d)
+ ideal_loading_bay_blue.polygon_list_3d.append(loading_bay_blue_polygon_points_3d)
+
+ ideal_target_list.append(ideal_loading_bay_blue)
+ training_target_list.append(TargetData('test_images/train_loading_bay_blue.png'))
+
+ # Create feature extractor
+ feature_extractor = tam.load_feature_extractor()
+
+ # Use webcam parameters for now
+ camera_params = camera_definition.web_cam_params
+
+ for ideal_target in ideal_target_list:
+ print("\nPreparing target for image %s" % ideal_target.image_filename)
+ ideal_target.extract_features(feature_extractor)
+ ideal_target.filter_keypoints_by_polygons()
+ ideal_target.compute_reprojection_maps()
+ ideal_target.keypoint_list_3d = ideal_target.project_keypoint_to_3d_by_polygon(
+ ideal_target.keypoint_list)
+
+ if VISUALIZE_KEYPOINTS:
+ for i in range(len(ideal_target.polygon_list)):
+ ideal_pts_tmp = np.asarray(ideal_target.polygon_list[i]).reshape(
+ -1, 2)
+ ideal_pts_3d_tmp = np.asarray(
+ ideal_target.polygon_list_3d[i]).reshape(-1, 3)
+ # We can only compute pose if we have at least 4 points
+ # Only matters for reprojection for visualization
+ # Keeping this code here, since it's helpful when testing
+ if (len(ideal_target.polygon_list[i]) >= 4):
+ img_copy = dtd.draw_polygon(ideal_target.image.copy(), ideal_target.polygon_list[i], (0,255,0), True)
+ dtd.visualize_reprojections(img_copy, ideal_pts_tmp, ideal_pts_3d_tmp, camera_params.camera_int.camera_matrix, camera_params.camera_int.distortion_coeffs)
+
+ for polygon in ideal_target.polygon_list:
+ img_copy = ideal_target.image.copy()
+ kp_in_poly2d = []
+ kp_in_poly3d = []
+ for kp, kp_3d in zip(ideal_target.keypoint_list,
+ ideal_target.keypoint_list_3d):
+ if dtd.point_in_polygons((kp.pt[0], kp.pt[1]), [polygon]):
+ kp_in_poly2d.append((kp.pt[0], kp.pt[1]))
+ kp_in_poly3d.append(kp_3d)
+
+ dtd.visualize_reprojections(
+ img_copy,
+ np.asarray(kp_in_poly2d).reshape(-1, 2),
+ np.asarray(kp_in_poly3d).reshape(
+ -1, 3), camera_params.camera_int.camera_matrix,
+ camera_params.camera_int.distortion_coeffs)
+
+ ###############
+ ### Compute 3D points on actual training images
+ ### TODO: Add code to do manual point selection
+ ###############
+ AUTO_PROJECTION = True
+
+ if AUTO_PROJECTION:
+ print("\n\nAuto projection of training keypoints to 3D using ideal images")
+ # Match the captured training image against the "ideal" training image
+ # and use those matches to pin down the 3D locations of the keypoints
+
+ for target_ind in range(len(training_target_list)):
+ # Assumes we have 1 ideal view for each training target
+ training_target = training_target_list[target_ind]
+ ideal_target = ideal_target_list[target_ind]
+
+ print("\nPreparing target for image %s" % training_target.image_filename)
+ # Extract keypoints and descriptors for model
+ training_target.extract_features(feature_extractor)
+
+ # Create matcher that we'll use to match with ideal
+ matcher = tam.train_matcher([training_target.descriptor_list])
+
+ matches_list = tam.compute_matches(matcher,
+ [training_target.descriptor_list],
+ [ideal_target.descriptor_list])
+
+ homography_list, matches_mask_list = tam.compute_homographies(
+ [training_target.keypoint_list], [ideal_target.keypoint_list],
+ matches_list)
+
+ for polygon in ideal_target.polygon_list:
+ ideal_pts_2d = np.asarray(np.float32(polygon)).reshape(-1, 1, 2)
+ H_inv = np.linalg.inv(homography_list[0])
+ # We use the ideal target's polygons to define the polygons on
+ # the training target
+ transformed_polygon = cv2.perspectiveTransform(ideal_pts_2d, H_inv)
+ training_target.polygon_list.append(transformed_polygon)
+
+ print("Started with %d keypoints" % len(training_target.keypoint_list))
+
+ training_target.keypoint_list, training_target.descriptor_list, rejected_keypoint_list, rejected_descriptor_list, _ = dtd.filter_keypoints_by_polygons(
+ training_target.keypoint_list, training_target.descriptor_list,
+ training_target.polygon_list)
+ print("After filtering by polygons, had %d keypoints" % len(
+ training_target.keypoint_list))
+ if VISUALIZE_KEYPOINTS:
+ tam.show_keypoints(training_target.image,
+ training_target.keypoint_list)
+
+ # Now comes the fun part
+ # Go through all my training keypoints to define 3D location using ideal
+ training_3d_list = []
+ for kp_ind in range(len(training_target.keypoint_list)):
+ # We're going to look for the first time this keypoint is in a polygon
+ found_3d_loc = False
+ # First, is it in the correct polygon
+ kp_loc = (training_target.keypoint_list[kp_ind].pt[0],
+ training_target.keypoint_list[kp_ind].pt[1])
+ for poly_ind in range(len(training_target.polygon_list)):
+ if dtd.point_in_polygons(
+ kp_loc, [training_target.polygon_list[poly_ind]
+ ]) and not found_3d_loc:
+ found_3d_loc = True
+ # If so, transform keypoint location to ideal using homography, and compute 3D
+ kp_loc_array = np.asarray(np.float32(kp_loc)).reshape(
+ -1, 1, 2)
+ training_2d_in_ideal = cv2.perspectiveTransform(
+ kp_loc_array, homography_list[0])
+ # Get 3D from this 2D point in ideal image
+ training_3d_pt = dtd.compute_3d_points(
+ training_2d_in_ideal,
+ ideal_target.reprojection_map_list[poly_ind])
+ training_3d_list.append(training_3d_pt)
+
+ training_target.keypoint_list_3d = np.asarray(
+ training_3d_list).reshape(-1, 1, 3)
+
+ if VISUALIZE_KEYPOINTS:
+ # Sanity check these:
+ img_copy = training_target.image.copy()
+ for polygon in training_target.polygon_list:
+ pts = polygon.astype(int).reshape(-1, 2)
+ img_copy = dtd.draw_polygon(img_copy, pts,
+ (255, 0, 0), True)
+ kp_tmp = np.asarray([
+ (kp.pt[0], kp.pt[1]) for kp in training_target.keypoint_list
+ ]).reshape(-1, 2)
+ dtd.visualize_reprojections(
+ img_copy, kp_tmp, training_target.keypoint_list_3d,
+ camera_params.camera_int.camera_matrix,
+ camera_params.camera_int.distortion_coeffs)
+
+ y2020_target_list = training_target_list
+ return y2020_target_list
+
+if __name__ == '__main__':
+ ap = argparse.ArgumentParser()
+ ap.add_argument("--visualize", help="Whether to visualize the results", default=False, action='store_true')
+ ap.add_argument("--no_use_bazel", help="Don't run using Bazel", default=True, action='store_false')
+ args = vars(ap.parse_args())
+
+ VISUALIZE_KEYPOINTS = args["visualize"]
+ if args["visualize"]:
+ print("Visualizing results")
+
+ USE_BAZEL = args["no_use_bazel"]
+ if args["no_use_bazel"]:
+ print("Running on command line (no Bazel)")
+
+ compute_target_definition()
+ pass
diff --git a/y2020/vision/tools/python_code/test_images/train_power_port_blue.png b/y2020/vision/tools/python_code/test_images/train_power_port_blue.png
index 6ade26f..a3a7597 100644
--- a/y2020/vision/tools/python_code/test_images/train_power_port_blue.png
+++ b/y2020/vision/tools/python_code/test_images/train_power_port_blue.png
Binary files differ
diff --git a/y2020/vision/tools/python_code/train_and_match.py b/y2020/vision/tools/python_code/train_and_match.py
index 8fed4c3..e5a7f5b 100644
--- a/y2020/vision/tools/python_code/train_and_match.py
+++ b/y2020/vision/tools/python_code/train_and_match.py
@@ -1,14 +1,8 @@
import cv2
import math
-from matplotlib import pyplot as plt
import numpy as np
-#import tkinter
-# pip install pillow
-#import PIL.Image, PIL.ImageTk
import time
-import field_display
-
### DEFINITIONS
MIN_MATCH_COUNT = 10 # 10 is min; more gives better matches
FEATURE_EXTRACTOR_NAME = 'SIFT'
@@ -285,7 +279,9 @@
# keypoint_list: List of opencv keypoints
def show_keypoints(image, keypoint_list):
ret_img = image.copy()
- ret_img = cv2.drawKeypoints(ret_img, keypoint_list, ret_img)
+ ret_img = cv2.drawKeypoints(ret_img, keypoint_list, ret_img,
+ flags=cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
cv2.imshow("Keypoints", ret_img)
cv2.waitKey(0)
return ret_img
+
diff --git a/y2020/vision/tools/python_code/transform_projection_test.py b/y2020/vision/tools/python_code/transform_projection_test.py
index b8d94fd..00f68bb 100644
--- a/y2020/vision/tools/python_code/transform_projection_test.py
+++ b/y2020/vision/tools/python_code/transform_projection_test.py
@@ -1,111 +1,90 @@
import cv2
import math
-from matplotlib import pyplot as plt
import numpy as np
import field_display
+import camera_definition
-# Assume image is 640x480 (VGA)
-num_pixels_x = 640
-num_pixels_y = 480
-
-# Camera center is 320, 240
-c_x = num_pixels_x / 2
-c_y = num_pixels_y / 2
-
-# Horiz. FOV is 54 degrees
-FOV_h = 54 * math.pi / 180.0 # (in radians)
-# Vert FOV is horizontal FOV scaled by aspect ratio (through tan function)
-FOV_v = math.atan(c_y / c_x * math.tan(FOV_h / 2.)) * 2
-
-# Assuming square pixels
-# focal length is (640/2)/tan(FOV_h/2)
-f_x = c_x / (math.tan(FOV_h / 2.))
-f_y = c_y / (math.tan(FOV_v / 2.))
+# Import camera from camera_definition
+camera_params = camera_definition.web_cam_params
+cam_mat = camera_params.camera_int.camera_matrix
+distortion_coeffs = camera_params.camera_int.distortion_coeffs
# Height of camera on robot above ground
cam_above_ground = 0.5
-
-# TODO: Should fix f_y entry below.
-cam_mat = np.array([[f_x, 0, c_x], [0, f_y, c_y], [0, 0, 1.]])
-
-# Use default distortion (i.e., none)
-distortion_coeffs = np.zeros((5, 1))
-
depth = 5.0 # meters
-R_w_tarj_mat = np.array([[0., 0., 1.], [-1, 0, 0], [0, -1., 0]])
-R_w_tarj, jac = cv2.Rodrigues(R_w_tarj_mat)
-T_w_tarj = np.array([[15.98 - depth, -4.10 + 2.36, cam_above_ground]]).T
+# Define camera location (cam) relative to origin (w)
+R_w_cam_mat = np.array([[0., 0., 1.], [-1, 0, 0], [0, -1., 0]])
+R_w_cam, jac = cv2.Rodrigues(R_w_cam_mat)
+T_w_cam = np.array([[15.98 - depth, -4.10 + 2.36, cam_above_ground]]).T
-img_ret = field_display.plot_bot_on_field(None, (0, 255, 0), T_w_tarj)
-#field_display.show_field(img_ret)
+img_ret = field_display.plot_bot_on_field(None, (0, 255, 0), T_w_cam)
# Create fake set of points relative to camera capture (target) frame
# at +/- 1 meter in x, 5 meter depth, and every 1 meter in y from 0 to 4m (above the camera, so negative y values)
-pts_3d_T_t = np.array([[-1., 0., depth], [1., 0., depth], [-1., -1., depth], [
- 1., -1., depth
-], [-1., -2., depth], [0., -2., depth], [1., -2., depth], [-1., -3., depth],
- [1., -3., depth], [-1., -4., depth], [1., -4., depth]])
+pts_3d_target = np.array(
+ [[-1., 0., depth], [1., 0., depth], [-1., -1., depth], [1., -1., depth],
+ [-1., -2., depth], [0., -2., depth], [1., -2., depth], [-1., -3., depth],
+ [1., -3., depth], [-1., -4., depth], [1., -4., depth]])
-# Ground truth shift of camera, to compute projections
-R_tarj_ci_gt = np.array([[0., 0.2, 0.2]]).T
+# Ground truth shift of camera from (cam) to (cam2), to compute projections
+R_cam_cam2_gt = np.array([[0., 0.2, 0.2]]).T
-R_tarj_ci_gt_mat, jac = cv2.Rodrigues(R_tarj_ci_gt)
+R_cam_cam2_gt_mat, jac = cv2.Rodrigues(R_cam_cam2_gt)
-T_tarj_ci_gt = np.array([[1., 2., -5.]]).T
+T_cam_cam2_gt = np.array([[1., 2., -5.]]).T
# To transform vectors, we apply the inverse transformation
-R_tarj_ci_gt_mat_inv = R_tarj_ci_gt_mat.T
-T_tarj_ci_gt_inv = -R_tarj_ci_gt_mat_inv.dot(T_tarj_ci_gt)
+R_cam_cam2_gt_mat_inv = R_cam_cam2_gt_mat.T
+T_cam_cam2_gt_inv = -R_cam_cam2_gt_mat_inv.dot(T_cam_cam2_gt)
-#pts_3d_T_t_shifted = (R_tarj_ci_gt_mat_inv.dot(pts_3d_T_t.T) + T_tarj_ci_gt_inv).T
+#pts_3d_target_shifted = (R_cam_cam2_gt_mat_inv.dot(pts_3d_target.T) + T_cam_cam2_gt_inv).T
# Now project from new position
# This was the manual way to do it-- use cv2.projectPoints instead
-#pts_proj_3d = cam_mat.dot(pts_3d_T_t_shifted.T).T
+#pts_proj_3d = cam_mat.dot(pts_3d_target_shifted.T).T
#pts_proj_2d = np.divide(pts_proj_3d[:,0:2],(pts_proj_3d[:,2].reshape(-1,1)))
-pts_proj_2d_ci, jac_2d = cv2.projectPoints(pts_3d_T_t, R_tarj_ci_gt_mat_inv,
- T_tarj_ci_gt_inv, cam_mat,
- distortion_coeffs)
-#print(pts_proj_2d_ci)
+pts_proj_2d_cam2, jac_2d = cv2.projectPoints(
+ pts_3d_target, R_cam_cam2_gt_mat_inv, T_cam_cam2_gt_inv, cam_mat,
+ distortion_coeffs)
# Now, solve for the pose using the original 3d points (pts_3d_T_t) and the projections from the new location
-retval, R_ci_tarj_est, T_ci_tarj_est, inliers = cv2.solvePnPRansac(
- pts_3d_T_t, pts_proj_2d_ci, cam_mat, distortion_coeffs)
+retval, R_cam2_cam_est, T_cam2_cam_est, inliers = cv2.solvePnPRansac(
+ pts_3d_target, pts_proj_2d_cam2, cam_mat, distortion_coeffs)
# This is the pose from camera to original target spot. We need to invert to get back to the pose we want
-R_tarj_ci_est_mat = cv2.Rodrigues(R_ci_tarj_est)[0].T
-T_tarj_ci_est = -R_tarj_ci_est_mat.dot(T_ci_tarj_est)
-R_tarj_ci_est = cv2.Rodrigues(R_tarj_ci_est_mat)[0]
+R_cam_cam2_est_mat = cv2.Rodrigues(R_cam2_cam_est)[0].T
+T_cam_cam2_est = -R_cam_cam2_est_mat.dot(T_cam2_cam_est)
+R_cam_cam2_est = cv2.Rodrigues(R_cam_cam2_est_mat)[0]
-print("Check:\n", "Rot error:\n", R_tarj_ci_gt - R_tarj_ci_est,
- "\nTrans error:\n", T_tarj_ci_gt - T_tarj_ci_est,
+print("Check:\n", "Rot error:\n", R_cam_cam2_gt - R_cam_cam2_est,
+ "\nTrans error:\n", T_cam_cam2_gt - T_cam_cam2_est,
"\nError is < 0.001 in R & T: ",
- np.linalg.norm(R_tarj_ci_gt - R_tarj_ci_est) < 0.001,
- np.linalg.norm(T_tarj_ci_gt - T_tarj_ci_est) < 0.001)
+ np.linalg.norm(R_cam_cam2_gt - R_cam_cam2_est) < 0.001, " & ",
+ np.linalg.norm(T_cam_cam2_gt - T_cam_cam2_est) < 0.001)
# Compute camera location in world coordinates
-R_w_ci, T_w_ci, d1, d2, d3, d4, d5, d6, d7, d8 = cv2.composeRT(
- R_tarj_ci_est, T_tarj_ci_est, R_w_tarj, T_w_tarj)
+R_w_cam2, T_w_cam2, d1, d2, d3, d4, d5, d6, d7, d8 = cv2.composeRT(
+ R_cam_cam2_est, T_cam_cam2_est, R_w_cam, T_w_cam)
print("Estimate in world coordinates")
-print("R, T:\n", R_w_ci, "\n", T_w_ci)
-img_ret = field_display.plot_bot_on_field(img_ret, (0, 255, 255), T_w_ci)
+print("R, T:\n", R_w_cam2, "\n", T_w_cam2)
+img_ret = field_display.plot_bot_on_field(img_ret, (0, 255, 255), T_w_cam2)
field_display.show_field(img_ret)
# Compute vector to target
# TODO: Likely better to do this from the homography, rather than the pose estimate...
T_w_target_pt = np.array([[15.98, -4.10 + 2.36, 2.0 - cam_above_ground]]).T
-vector_to_target = T_w_target_pt - T_w_ci
-d_ci_target = np.linalg.norm(vector_to_target)
-phi_ci_target = math.atan2(vector_to_target[1][0], vector_to_target[0][0])
+vector_to_target = T_w_target_pt - T_w_cam2
+d_cam2_target = np.linalg.norm(vector_to_target)
+phi_cam2_target = math.atan2(vector_to_target[1][0], vector_to_target[0][0])
print("Vector to target (from cam frame):\n", vector_to_target,
- "\nDistance to target: ", d_ci_target, "\nAngle to target (deg): ",
- phi_ci_target * 180. / math.pi)
+ "\nDistance to target: ", d_cam2_target, "\nAngle to target (deg): ",
+ phi_cam2_target * 180. / math.pi)
-img_ret = field_display.plot_line_on_field(img_ret, (255, 255, 0), T_w_ci,
+img_ret = field_display.plot_line_on_field(img_ret, (255, 255, 0), T_w_cam2,
T_w_target_pt)
field_display.show_field(img_ret)
diff --git a/y2020/vision/tools/python_code/usb_camera_stream.py b/y2020/vision/tools/python_code/usb_camera_stream.py
index 42cb1f1..5d3ae91 100644
--- a/y2020/vision/tools/python_code/usb_camera_stream.py
+++ b/y2020/vision/tools/python_code/usb_camera_stream.py
@@ -7,7 +7,7 @@
print("Could not open video device")
quit()
-while (True):
+while True:
# Capture frame-by-frame
ret, frame = cap.read()
diff --git a/y2020/www/BUILD b/y2020/www/BUILD
index 146456f..c0daa07 100644
--- a/y2020/www/BUILD
+++ b/y2020/www/BUILD
@@ -10,6 +10,7 @@
deps = [
"//aos/network/www:proxy",
"//y2020/vision:vision_ts_fbs",
+ "//y2020/vision/sift:sift_ts_fbs",
],
visibility = ["//y2020:__subpackages__"],
)
diff --git a/y2020/www/image_handler.ts b/y2020/www/image_handler.ts
index abaf831..802e448 100644
--- a/y2020/www/image_handler.ts
+++ b/y2020/www/image_handler.ts
@@ -1,22 +1,27 @@
-import {frc971} from 'y2020/vision/vision_generated';
+import {CameraImage} from 'y2020/vision/vision_generated';
export class ImageHandler {
private canvas = document.createElement('canvas');
+ private imageBuffer: Uint8ClampedArray|null = null;
+ private imageTimestamp: flatbuffers.Long|null = null;
+ private result: fr971.vision.ImageMatchResult|null = null;
+ private resultTimestamp: flatbuffers.Long|null = null;
constructor() {
document.body.appendChild(this.canvas);
}
- handleImage(data: Uint8Array) {
+ handleImage(data: Uint8Array): void {
const fbBuffer = new flatbuffers.ByteBuffer(data);
- const image = frc971.vision.CameraImage.getRootAsCameraImage(fbBuffer);
+ const image = CameraImage.getRootAsCameraImage(fbBuffer);
+ this.imageTimestamp = image.monotonicTimestampNs();
const width = image.cols();
const height = image.rows();
if (width === 0 || height === 0) {
return;
}
- const imageBuffer = new Uint8ClampedArray(width * height * 4); // RGBA
+ this.imageBuffer = new Uint8ClampedArray(width * height * 4); // RGBA
// Read four bytes (YUYV) from the data and transform into two pixels of
// RGBA for canvas
@@ -46,16 +51,47 @@
}
}
+ draw();
+ }
+
+ handleImageMetadata(data: Uint8Array): void {
+ const fbBuffer = new flatbuffers.ByteBuffer(data);
+ this.result = frc971.vision.ImageMatchResult.getRootAsImageMatchResult(fbBuffer);
+ this.resultTimestamp = result.imageMonotonicTimestampNs();
+ draw();
+ }
+
+ draw(): void {
+ if (imageTimestamp.low !== resultTimestamp.low ||
+ imageTimestamp.high !== resultTimestamp.high) {
+ return;
+ }
const ctx = this.canvas.getContext('2d');
this.canvas.width = width;
this.canvas.height = height;
const idata = ctx.createImageData(width, height);
- idata.data.set(imageBuffer);
+ idata.data.set(this.imageBuffer);
ctx.putImageData(idata, 0, 0);
+ ctx.beginPath();
+ for (const feature of this.result.getFeatures()) {
+ // Based on OpenCV drawKeypoint.
+ ctx.arc(feature.x, feature.y, feature.size, 0, 2 * Math.PI);
+ ctx.moveTo(feature.x, feature.y);
+ // TODO(alex): check that angle is correct (0?, direction?)
+ const angle = feature.angle * Math.PI / 180;
+ ctx.lineTo(
+ feature.x + feature.radius * cos(angle),
+ feature.y + feature.radius * sin(angle));
+ }
+ ctx.stroke();
}
- getId() {
- return frc971.vision.CameraImage.getFullyQualifiedName();
+ getId(): string {
+ return CameraImage.getFullyQualifiedName();
+ }
+
+ getResultId(): string {
+ return frc971.vision.ImageMatchResult.getFullyQualifiedName();
}
}
diff --git a/y2020/www/main.ts b/y2020/www/main.ts
index 7831713..d414eac 100644
--- a/y2020/www/main.ts
+++ b/y2020/www/main.ts
@@ -9,3 +9,5 @@
const iHandler = new ImageHandler();
conn.addHandler(iHandler.getId(), (data) => iHandler.handleImage(data));
+conn.addHandler(
+ iHandler.getResultId(), (data) => iHandler.handleImageMetadata(data));
