Merge changes Ic4474429,Ic57eb3fa
* changes:
Remove mallocs from 2020 aimer.
Remove more mallocs when realtime
diff --git a/BUILD b/BUILD
index a568e20..413620d 100644
--- a/BUILD
+++ b/BUILD
@@ -35,6 +35,8 @@
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/refresh_match_list_response //scouting/webserver/requests/messages:refresh_match_list_response_go_fbs
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_shift_schedule //scouting/webserver/requests/messages:request_shift_schedule_go_fbs
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_shift_schedule_response //scouting/webserver/requests/messages:request_shift_schedule_response_go_fbs
+# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_actions //scouting/webserver/requests/messages:submit_actions_go_fbs
+# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_actions_response //scouting/webserver/requests/messages:submit_actions_response_go_fbs
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_shift_schedule //scouting/webserver/requests/messages:submit_shift_schedule_go_fbs
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_shift_schedule_response //scouting/webserver/requests/messages:submit_shift_schedule_response_go_fbs
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_driver_ranking //scouting/webserver/requests/messages:submit_driver_ranking_go_fbs
diff --git a/aos/events/logging/log_edit.cc b/aos/events/logging/log_edit.cc
index e3cfc3a..dca56bb 100644
--- a/aos/events/logging/log_edit.cc
+++ b/aos/events/logging/log_edit.cc
@@ -51,7 +51,10 @@
aos::logger::DetachedBufferWriter buffer_writer(
FLAGS_logfile,
std::make_unique<aos::logger::DummyEncoder>(FLAGS_max_message_size));
- buffer_writer.QueueSpan(header.span());
+ {
+ aos::logger::DataEncoder::SpanCopier coppier(header.span());
+ buffer_writer.CopyMessage(&coppier, aos::monotonic_clock::min_time);
+ }
while (true) {
absl::Span<const uint8_t> msg_data = span_reader.ReadMessage();
@@ -59,7 +62,10 @@
break;
}
- buffer_writer.QueueSpan(msg_data);
+ {
+ aos::logger::DataEncoder::SpanCopier coppier(msg_data);
+ buffer_writer.CopyMessage(&coppier, aos::monotonic_clock::min_time);
+ }
}
} else {
aos::logger::MessageReader reader(FLAGS_logfile);
diff --git a/aos/events/logging/log_namer.cc b/aos/events/logging/log_namer.cc
index 120bd79..c0c7c73 100644
--- a/aos/events/logging/log_namer.cc
+++ b/aos/events/logging/log_namer.cc
@@ -248,7 +248,8 @@
header, {.multi_line = false, .max_vector_size = 100});
CHECK(writer);
- writer->QueueSpan(header.span());
+ DataEncoder::SpanCopier coppier(header.span());
+ writer->CopyMessage(&coppier, aos::monotonic_clock::now());
header_written_ = true;
monotonic_start_time_ = log_namer_->monotonic_start_time(
node_index_, state_[node_index_].boot_uuid);
@@ -606,7 +607,8 @@
std::make_unique<DetachedBufferWriter>(
filename, encoder_factory_(header->span().size()));
- writer->QueueSpan(header->span());
+ DataEncoder::SpanCopier coppier(header->span());
+ writer->CopyMessage(&coppier, aos::monotonic_clock::now());
if (!writer->ran_out_of_space()) {
all_filenames_.emplace_back(
diff --git a/aos/events/logging/logfile_utils.cc b/aos/events/logging/logfile_utils.cc
index ab56356..90220c8 100644
--- a/aos/events/logging/logfile_utils.cc
+++ b/aos/events/logging/logfile_utils.cc
@@ -129,27 +129,12 @@
return *this;
}
-void DetachedBufferWriter::QueueSpan(absl::Span<const uint8_t> span) {
+void DetachedBufferWriter::CopyMessage(DataEncoder::Copier *coppier,
+ aos::monotonic_clock::time_point now) {
if (ran_out_of_space_) {
// We don't want any later data to be written after space becomes
// available, so refuse to write anything more once we've dropped data
// because we ran out of space.
- VLOG(1) << "Ignoring span: " << span.size();
- return;
- }
-
- if (!encoder_->HasSpace(span.size())) {
- Flush();
- CHECK(encoder_->HasSpace(span.size()));
- }
- DataEncoder::SpanCopier coppier(span);
- encoder_->Encode(&coppier);
- FlushAtThreshold(aos::monotonic_clock::now());
-}
-
-void DetachedBufferWriter::CopyMessage(DataEncoder::Copier *coppier,
- aos::monotonic_clock::time_point now) {
- if (ran_out_of_space_) {
return;
}
diff --git a/aos/events/logging/logfile_utils.h b/aos/events/logging/logfile_utils.h
index 346a36e..ca36ace 100644
--- a/aos/events/logging/logfile_utils.h
+++ b/aos/events/logging/logfile_utils.h
@@ -76,9 +76,6 @@
void CopyMessage(DataEncoder::Copier *coppier,
aos::monotonic_clock::time_point now);
- // Queues up data in span. May copy or may write it to disk immediately.
- void QueueSpan(absl::Span<const uint8_t> span);
-
// Indicates we got ENOSPC when trying to write. After this returns true, no
// further data is written.
bool ran_out_of_space() const { return ran_out_of_space_; }
diff --git a/aos/events/logging/logfile_utils_out_of_space_test_runner.cc b/aos/events/logging/logfile_utils_out_of_space_test_runner.cc
index afc4d77..719eb6e 100644
--- a/aos/events/logging/logfile_utils_out_of_space_test_runner.cc
+++ b/aos/events/logging/logfile_utils_out_of_space_test_runner.cc
@@ -22,10 +22,14 @@
FLAGS_tmpfs + "/file",
std::make_unique<aos::logger::DummyEncoder>(data.size()));
for (int i = 0; i < 8; ++i) {
- writer.QueueSpan(data);
+ aos::logger::DataEncoder::SpanCopier coppier(data);
+ writer.CopyMessage(&coppier, aos::monotonic_clock::now());
CHECK(!writer.ran_out_of_space()) << ": " << i;
}
- writer.QueueSpan(data);
+ {
+ aos::logger::DataEncoder::SpanCopier coppier(data);
+ writer.CopyMessage(&coppier, aos::monotonic_clock::now());
+ }
CHECK(writer.ran_out_of_space());
writer.acknowledge_out_of_space();
}
diff --git a/aos/events/logging/logfile_utils_test.cc b/aos/events/logging/logfile_utils_test.cc
index f453798..b3a9bbd 100644
--- a/aos/events/logging/logfile_utils_test.cc
+++ b/aos/events/logging/logfile_utils_test.cc
@@ -39,6 +39,10 @@
void WriteSizedFlatbuffer(flatbuffers::DetachedBuffer &&buffer) {
QueueSpan(absl::Span<const uint8_t>(buffer.data(), buffer.size()));
}
+ void QueueSpan(absl::Span<const uint8_t> buffer) {
+ DataEncoder::SpanCopier coppier(buffer);
+ CopyMessage(&coppier, monotonic_clock::now());
+ }
};
// Creates a size prefixed flatbuffer from json.
diff --git a/aos/network/www/plotter.ts b/aos/network/www/plotter.ts
index 89b360c..17bffa3 100644
--- a/aos/network/www/plotter.ts
+++ b/aos/network/www/plotter.ts
@@ -342,8 +342,11 @@
// Make sure both have text in the right spot. Don't be too picky since
// nothing should really be changing here, and it's handy to let the
// user edit the HTML for testing.
- if (this.legend.children[child].lastChild.textContent.length == 0 &&
- line.label().length != 0) {
+ let textdiv = this.legend.children[child].lastChild;
+ let canvas = this.legend.children[child].firstChild;
+ if ((textdiv.textContent.length == 0 && line.label().length != 0) ||
+ (textdiv as HTMLDivElement).offsetHeight !=
+ (canvas as HTMLCanvasElement).height) {
needsUpdate = true;
break;
}
diff --git a/frc971/vision/v4l2_reader.cc b/frc971/vision/v4l2_reader.cc
index 7c0546f..2553003 100644
--- a/frc971/vision/v4l2_reader.cc
+++ b/frc971/vision/v4l2_reader.cc
@@ -327,7 +327,8 @@
: V4L2ReaderBase(event_loop, device_name),
epoll_(epoll),
image_sensor_fd_(open(image_sensor_subdev.c_str(), O_RDWR | O_NONBLOCK)),
- buffer_requeuer_([this](int buffer) { EnqueueBuffer(buffer); }, 20) {
+ buffer_requeuer_([this](int buffer) { EnqueueBuffer(buffer); },
+ kEnqueueFifoPriority) {
PCHECK(image_sensor_fd_.get() != -1)
<< " Failed to open device " << device_name;
StreamOn();
diff --git a/frc971/vision/v4l2_reader.h b/frc971/vision/v4l2_reader.h
index 31c0888..22e4367 100644
--- a/frc971/vision/v4l2_reader.h
+++ b/frc971/vision/v4l2_reader.h
@@ -185,6 +185,8 @@
aos::ScopedFD image_sensor_fd_;
+ static constexpr int kEnqueueFifoPriority = 1;
+
aos::util::ThreadedConsumer<int, kNumberBuffers> buffer_requeuer_;
};
diff --git a/scouting/db/db.go b/scouting/db/db.go
index 85e3d98..8f043de 100644
--- a/scouting/db/db.go
+++ b/scouting/db/db.go
@@ -66,6 +66,34 @@
CollectedBy string
}
+type Stats2023 struct {
+ TeamNumber string `gorm:"primaryKey"`
+ MatchNumber int32 `gorm:"primaryKey"`
+ SetNumber int32 `gorm:"primaryKey"`
+ CompLevel string `gorm:"primaryKey"`
+ StartingQuadrant int32
+ LowCubesAuto, MiddleCubesAuto, HighCubesAuto, CubesDroppedAuto int32
+ LowConesAuto, MiddleConesAuto, HighConesAuto, ConesDroppedAuto int32
+ LowCubes, MiddleCubes, HighCubes, CubesDropped int32
+ LowCones, MiddleCones, HighCones, ConesDropped int32
+ AvgCycle int32
+ // The username of the person who collected these statistics.
+ // "unknown" if submitted without logging in.
+ // Empty if the stats have not yet been collected.
+ CollectedBy string
+}
+
+type Action struct {
+ TeamNumber string `gorm:"primaryKey"`
+ MatchNumber int32 `gorm:"primaryKey"`
+ SetNumber int32 `gorm:"primaryKey"`
+ CompLevel string `gorm:"primaryKey"`
+ CompletedAction []byte
+ // This contains a serialized scouting.webserver.requests.ActionType flatbuffer.
+ TimeStamp int32 `gorm:"primaryKey"`
+ CollectedBy string
+}
+
type NotesData struct {
ID uint `gorm:"primaryKey"`
TeamNumber int32
@@ -113,7 +141,7 @@
return nil, errors.New(fmt.Sprint("Failed to connect to postgres: ", err))
}
- err = database.AutoMigrate(&Match{}, &Shift{}, &Stats{}, &NotesData{}, &Ranking{}, &DriverRankingData{})
+ err = database.AutoMigrate(&Match{}, &Shift{}, &Stats{}, &Stats2023{}, &Action{}, &NotesData{}, &Ranking{}, &DriverRankingData{})
if err != nil {
database.Delete()
return nil, errors.New(fmt.Sprint("Failed to create/migrate tables: ", err))
@@ -148,6 +176,13 @@
return result.Error
}
+func (database *Database) AddAction(a Action) error {
+ result := database.Clauses(clause.OnConflict{
+ UpdateAll: true,
+ }).Create(&a)
+ return result.Error
+}
+
func (database *Database) AddToStats(s Stats) error {
matches, err := database.queryMatches(s.TeamNumber)
if err != nil {
@@ -176,6 +211,28 @@
return result.Error
}
+func (database *Database) AddToStats2023(s Stats2023) error {
+ matches, err := database.QueryMatchesString(s.TeamNumber)
+ if err != nil {
+ return err
+ }
+ foundMatch := false
+ for _, match := range matches {
+ if match.MatchNumber == s.MatchNumber {
+ foundMatch = true
+ break
+ }
+ }
+ if !foundMatch {
+ return errors.New(fmt.Sprint(
+ "Failed to find team ", s.TeamNumber,
+ " in match ", s.MatchNumber, " in the schedule."))
+ }
+
+ result := database.Create(&s)
+ return result.Error
+}
+
func (database *Database) AddOrUpdateRankings(r Ranking) error {
result := database.Clauses(clause.OnConflict{
UpdateAll: true,
@@ -207,6 +264,12 @@
return shifts, result.Error
}
+func (database *Database) ReturnActions() ([]Action, error) {
+ var actions []Action
+ result := database.Find(&actions)
+ return actions, result.Error
+}
+
// Packs the stats. This really just consists of taking the individual auto
// ball booleans and turning them into an array. The individual booleans are
// cleared so that they don't affect struct comparisons.
@@ -249,6 +312,14 @@
return matches, result.Error
}
+func (database *Database) QueryMatchesString(teamNumber_ string) ([]Match, error) {
+ var matches []Match
+ result := database.
+ Where("r1 = $1 OR r2 = $1 OR r3 = $1 OR b1 = $1 OR b2 = $1 OR b3 = $1", teamNumber_).
+ Find(&matches)
+ return matches, result.Error
+}
+
func (database *Database) QueryAllShifts(matchNumber_ int) ([]Shift, error) {
var shifts []Shift
result := database.Where("match_number = ?", matchNumber_).Find(&shifts)
@@ -265,6 +336,13 @@
return stats, result.Error
}
+func (database *Database) QueryActions(teamNumber_ int) ([]Action, error) {
+ var actions []Action
+ result := database.
+ Where("team_number = ?", teamNumber_).Find(&actions)
+ return actions, result.Error
+}
+
func (database *Database) QueryNotes(TeamNumber int32) ([]string, error) {
var rawNotes []NotesData
result := database.Where("team_number = ?", TeamNumber).Find(&rawNotes)
diff --git a/scouting/db/db_test.go b/scouting/db/db_test.go
index 47a0a90..9e85651 100644
--- a/scouting/db/db_test.go
+++ b/scouting/db/db_test.go
@@ -623,6 +623,54 @@
}
}
+func TestReturnActionsDB(t *testing.T) {
+ fixture := createDatabase(t)
+ defer fixture.TearDown()
+ correct := []Action{
+ Action{
+ TeamNumber: "1235", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), TimeStamp: 0000, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1236", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), TimeStamp: 0321, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1237", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), TimeStamp: 0222, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1238", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), TimeStamp: 0110, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1239", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), TimeStamp: 0004, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1233", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), TimeStamp: 0004, CollectedBy: "",
+ },
+ }
+
+ err := fixture.db.AddToMatch(Match{
+ MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ R1: 1235, R2: 1236, R3: 1237, B1: 1238, B2: 1239, B3: 1233})
+ check(t, err, "Failed to add match")
+
+ for i := 0; i < len(correct); i++ {
+ err = fixture.db.AddAction(correct[i])
+ check(t, err, fmt.Sprint("Failed to add to actions ", i))
+ }
+
+ got, err := fixture.db.ReturnActions()
+ check(t, err, "Failed ReturnActions()")
+
+ if !reflect.DeepEqual(correct, got) {
+ t.Errorf("Got %#v,\nbut expected %#v.", got, correct)
+ }
+}
+
func TestRankingsDbUpdate(t *testing.T) {
fixture := createDatabase(t)
defer fixture.TearDown()
diff --git a/scouting/webserver/requests/BUILD b/scouting/webserver/requests/BUILD
index 8cc59a9..9eb207d 100644
--- a/scouting/webserver/requests/BUILD
+++ b/scouting/webserver/requests/BUILD
@@ -24,6 +24,8 @@
"//scouting/webserver/requests/messages:request_notes_for_team_response_go_fbs",
"//scouting/webserver/requests/messages:request_shift_schedule_go_fbs",
"//scouting/webserver/requests/messages:request_shift_schedule_response_go_fbs",
+ "//scouting/webserver/requests/messages:submit_actions_go_fbs",
+ "//scouting/webserver/requests/messages:submit_actions_response_go_fbs",
"//scouting/webserver/requests/messages:submit_data_scouting_go_fbs",
"//scouting/webserver/requests/messages:submit_data_scouting_response_go_fbs",
"//scouting/webserver/requests/messages:submit_driver_ranking_go_fbs",
diff --git a/scouting/webserver/requests/messages/BUILD b/scouting/webserver/requests/messages/BUILD
index 57b7ae3..2e118d5 100644
--- a/scouting/webserver/requests/messages/BUILD
+++ b/scouting/webserver/requests/messages/BUILD
@@ -25,6 +25,8 @@
"submit_shift_schedule_response",
"submit_driver_ranking",
"submit_driver_ranking_response",
+ "submit_actions",
+ "submit_actions_response",
)
filegroup(
diff --git a/scouting/webserver/requests/messages/request_data_scouting_response.fbs b/scouting/webserver/requests/messages/request_data_scouting_response.fbs
index 071a848..6048210 100644
--- a/scouting/webserver/requests/messages/request_data_scouting_response.fbs
+++ b/scouting/webserver/requests/messages/request_data_scouting_response.fbs
@@ -49,4 +49,4 @@
stats_list:[Stats] (id:0);
}
-root_type RequestDataScoutingResponse;
+root_type RequestDataScoutingResponse;
\ No newline at end of file
diff --git a/scouting/webserver/requests/messages/submit_actions.fbs b/scouting/webserver/requests/messages/submit_actions.fbs
new file mode 100644
index 0000000..9d9efa4
--- /dev/null
+++ b/scouting/webserver/requests/messages/submit_actions.fbs
@@ -0,0 +1,53 @@
+namespace scouting.webserver.requests;
+
+table StartMatchAction {
+ position:int (id:0);
+}
+
+enum ObjectType: short {
+ kCube,
+ kCone
+}
+
+enum ScoreLevel: short {
+ kLow,
+ kMiddle,
+ kHigh
+}
+
+table PickupObjectAction {
+ object_type:ObjectType (id:0);
+ auto:bool (id:1);
+}
+
+table PlaceObjectAction {
+ object_type:ObjectType (id:0);
+ score_level:ScoreLevel (id:1);
+ auto:bool (id:2);
+}
+
+table RobotDeathAction {
+ robot_on:bool (id:0);
+}
+
+table EndMatchAction {
+}
+
+union ActionType {
+ StartMatchAction,
+ PickupObjectAction,
+ PlaceObjectAction,
+ RobotDeathAction,
+ EndMatchAction
+}
+
+table Action {
+ timestamp:int (id:0);
+ action_taken:ActionType (id:2);
+}
+
+table SubmitActions {
+ actions_list:[Action] (id:0);
+}
+
+root_type SubmitActions;
\ No newline at end of file
diff --git a/scouting/webserver/requests/messages/submit_actions_response.fbs b/scouting/webserver/requests/messages/submit_actions_response.fbs
new file mode 100644
index 0000000..4079914
--- /dev/null
+++ b/scouting/webserver/requests/messages/submit_actions_response.fbs
@@ -0,0 +1,7 @@
+namespace scouting.webserver.requests;
+
+table SubmitActionsResponse {
+ // empty response
+}
+
+root_type SubmitActionsResponse;
\ No newline at end of file
diff --git a/scouting/webserver/requests/requests.go b/scouting/webserver/requests/requests.go
index 76a26de..99b5459 100644
--- a/scouting/webserver/requests/requests.go
+++ b/scouting/webserver/requests/requests.go
@@ -27,6 +27,8 @@
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_notes_for_team_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_shift_schedule"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_shift_schedule_response"
+ "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_actions"
+ "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_actions_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_data_scouting"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_data_scouting_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_driver_ranking"
@@ -61,6 +63,8 @@
type SubmitShiftScheduleResponseT = submit_shift_schedule_response.SubmitShiftScheduleResponseT
type SubmitDriverRanking = submit_driver_ranking.SubmitDriverRanking
type SubmitDriverRankingResponseT = submit_driver_ranking_response.SubmitDriverRankingResponseT
+type SubmitActions = submit_actions.SubmitActions
+type SubmitActionsResponseT = submit_actions_response.SubmitActionsResponseT
// The interface we expect the database abstraction to conform to.
// We use an interface here because it makes unit testing easier.
diff --git a/y2023/control_loops/python/BUILD b/y2023/control_loops/python/BUILD
index a2ea6d8..9b1746a 100644
--- a/y2023/control_loops/python/BUILD
+++ b/y2023/control_loops/python/BUILD
@@ -35,7 +35,8 @@
name = "graph_edit",
srcs = [
"graph_edit.py",
- "graph_generate.py",
+ "graph_paths.py",
+ "graph_tools.py",
],
legacy_create_init = False,
target_compatible_with = ["@platforms//cpu:x86_64"],
diff --git a/y2023/control_loops/python/graph_edit.py b/y2023/control_loops/python/graph_edit.py
index 7b6179c..39cd814 100644
--- a/y2023/control_loops/python/graph_edit.py
+++ b/y2023/control_loops/python/graph_edit.py
@@ -11,21 +11,17 @@
gi.require_version('Gtk', '3.0')
from gi.repository import Gdk, Gtk
import cairo
-import graph_generate
-from graph_generate import XYSegment, AngleSegment, to_theta, to_xy, alpha_blend
-from graph_generate import back_to_xy_loop, subdivide_theta, to_theta_loop
-from graph_generate import l1, l2, joint_center
+from graph_tools import XYSegment, AngleSegment, to_theta, to_xy, alpha_blend, draw_lines
+from graph_tools import back_to_xy_loop, subdivide_theta, to_theta_loop, px
+from graph_tools import l1, l2, joint_center
+import graph_paths
-from frc971.control_loops.python.basic_window import OverrideMatrix, identity, quit_main_loop, set_color
+from frc971.control_loops.python.basic_window import quit_main_loop, set_color
import shapely
from shapely.geometry import Polygon
-def px(cr):
- return OverrideMatrix(cr, identity)
-
-
def draw_px_cross(cr, length_px):
"""Draws a cross with fixed dimensions in pixel space."""
with px(cr):
@@ -81,15 +77,6 @@
t2_max = numpy.pi * 3.0 / 4.0
-# Draw lines to cr + stroke.
-def draw_lines(cr, lines):
- cr.move_to(lines[0][0], lines[0][1])
- for pt in lines[1:]:
- cr.line_to(pt[0], pt[1])
- with px(cr):
- cr.stroke()
-
-
# Rotate a rasterized loop such that it aligns to when the parameters loop
def rotate_to_jump_point(points):
last_pt = points[0]
@@ -219,9 +206,6 @@
event.x = o_x
event.y = o_y
- def do_button_press(self, event):
- pass
-
def _do_configure(self, event):
self.window_shape = (event.width, event.height)
@@ -270,7 +254,32 @@
self.extents_y_max - self.extents_y_min)
cr.fill()
- if not self.theta_version:
+ if self.theta_version:
+ # Draw a filled white rectangle.
+ set_color(cr, palette["WHITE"])
+ cr.rectangle(-numpy.pi, -numpy.pi, numpy.pi * 2.0, numpy.pi * 2.0)
+ cr.fill()
+
+ set_color(cr, palette["BLUE"])
+ for i in range(-6, 6):
+ cr.move_to(-40, -40 + i * numpy.pi)
+ cr.line_to(40, 40 + i * numpy.pi)
+ with px(cr):
+ cr.stroke()
+
+ set_color(cr, Color(0.5, 0.5, 1.0))
+ draw_lines(cr, lines_theta)
+
+ set_color(cr, Color(0.0, 1.0, 0.2))
+ cr.move_to(self.last_pos[0], self.last_pos[1])
+ draw_px_cross(cr, 5)
+
+ c_pt, dist = closest_segment(lines_theta, self.last_pos)
+ print("dist:", dist, c_pt, self.last_pos)
+ set_color(cr, palette["CYAN"])
+ cr.move_to(c_pt[0], c_pt[1])
+ draw_px_cross(cr, 5)
+ else:
# Draw a filled white rectangle.
set_color(cr, palette["WHITE"])
cr.rectangle(-2.0, -2.0, 4.0, 4.0)
@@ -285,24 +294,7 @@
2.0 * numpy.pi)
with px(cr):
cr.stroke()
- else:
- # Draw a filled white rectangle.
- set_color(cr, palette["WHITE"])
- cr.rectangle(-numpy.pi, -numpy.pi, numpy.pi * 2.0, numpy.pi * 2.0)
- cr.fill()
- if self.theta_version:
- set_color(cr, palette["BLUE"])
- for i in range(-6, 6):
- cr.move_to(-40, -40 + i * numpy.pi)
- cr.line_to(40, 40 + i * numpy.pi)
- with px(cr):
- cr.stroke()
-
- if self.theta_version:
- set_color(cr, Color(0.5, 0.5, 1.0))
- draw_lines(cr, lines_theta)
- else:
set_color(cr, Color(0.5, 1.0, 1.0))
draw_lines(cr, lines1)
draw_lines(cr, lines2)
@@ -333,7 +325,6 @@
with px(cr):
cr.stroke()
- if not self.theta_version:
theta1, theta2 = to_theta(self.last_pos,
self.circular_index_select)
x, y = joint_center[0], joint_center[1]
@@ -352,17 +343,6 @@
set_color(cr, Color(0.0, 1.0, 0.2))
draw_px_cross(cr, 20)
- if self.theta_version:
- set_color(cr, Color(0.0, 1.0, 0.2))
- cr.move_to(self.last_pos[0], self.last_pos[1])
- draw_px_cross(cr, 5)
-
- c_pt, dist = closest_segment(lines_theta, self.last_pos)
- print("dist:", dist, c_pt, self.last_pos)
- set_color(cr, palette["CYAN"])
- cr.move_to(c_pt[0], c_pt[1])
- draw_px_cross(cr, 5)
-
set_color(cr, Color(0.0, 0.5, 1.0))
for segment in self.segments:
color = [0, random.random(), 1]
@@ -491,5 +471,5 @@
silly = Silly()
-silly.segments = graph_generate.segments
+silly.segments = graph_paths.segments
basic_window.RunApp()
diff --git a/y2023/control_loops/python/graph_generate.py b/y2023/control_loops/python/graph_generate.py
deleted file mode 100644
index 046b9dd..0000000
--- a/y2023/control_loops/python/graph_generate.py
+++ /dev/null
@@ -1,798 +0,0 @@
-import numpy
-
-# joint_center in x-y space.
-joint_center = (-0.299, 0.299)
-
-# Joint distances (l1 = "proximal", l2 = "distal")
-l1 = 46.25 * 0.0254
-l2 = 43.75 * 0.0254
-
-
-# Convert from x-y coordinates to theta coordinates.
-# orientation is a bool. This orientation is circular_index mod 2.
-# where circular_index is the circular index, or the position in the
-# "hyperextension" zones. "cross_point" allows shifting the place where
-# it rounds the result so that it draws nicer (no other functional differences).
-def to_theta(pt, circular_index, cross_point=-numpy.pi):
- orient = (circular_index % 2) == 0
- x = pt[0]
- y = pt[1]
- x -= joint_center[0]
- y -= joint_center[1]
- l3 = numpy.hypot(x, y)
- t3 = numpy.arctan2(y, x)
- theta1 = numpy.arccos((l1**2 + l3**2 - l2**2) / (2 * l1 * l3))
-
- if orient:
- theta1 = -theta1
- theta1 += t3
- theta1 = (theta1 - cross_point) % (2 * numpy.pi) + cross_point
- theta2 = numpy.arctan2(y - l1 * numpy.sin(theta1),
- x - l1 * numpy.cos(theta1))
- return numpy.array((theta1, theta2))
-
-
-# Simple trig to go back from theta1, theta2 to x-y
-def to_xy(theta1, theta2):
- x = numpy.cos(theta1) * l1 + numpy.cos(theta2) * l2 + joint_center[0]
- y = numpy.sin(theta1) * l1 + numpy.sin(theta2) * l2 + joint_center[1]
- orient = ((theta2 - theta1) % (2.0 * numpy.pi)) < numpy.pi
- return (x, y, orient)
-
-
-def get_circular_index(theta):
- return int(numpy.floor((theta[1] - theta[0]) / numpy.pi))
-
-
-def get_xy(theta):
- theta1 = theta[0]
- theta2 = theta[1]
- x = numpy.cos(theta1) * l1 + numpy.cos(theta2) * l2 + joint_center[0]
- y = numpy.sin(theta1) * l1 + numpy.sin(theta2) * l2 + joint_center[1]
- return numpy.array((x, y))
-
-
-# Draw a list of lines to a cairo context.
-def draw_lines(cr, lines):
- cr.move_to(lines[0][0], lines[0][1])
- for pt in lines[1:]:
- cr.line_to(pt[0], pt[1])
-
-
-max_dist = 0.01
-max_dist_theta = numpy.pi / 64
-xy_end_circle_size = 0.01
-theta_end_circle_size = 0.07
-
-
-# Subdivide in theta space.
-def subdivide_theta(lines):
- out = []
- last_pt = lines[0]
- out.append(last_pt)
- for n_pt in lines[1:]:
- for pt in subdivide(last_pt, n_pt, max_dist_theta):
- out.append(pt)
- last_pt = n_pt
-
- return out
-
-
-# subdivide in xy space.
-def subdivide_xy(lines, max_dist=max_dist):
- out = []
- last_pt = lines[0]
- out.append(last_pt)
- for n_pt in lines[1:]:
- for pt in subdivide(last_pt, n_pt, max_dist):
- out.append(pt)
- last_pt = n_pt
-
- return out
-
-
-def to_theta_with_ci(pt, circular_index):
- return to_theta_with_circular_index(pt[0], pt[1], circular_index)
-
-
-# to_theta, but distinguishes between
-def to_theta_with_circular_index(x, y, circular_index):
- theta1, theta2 = to_theta((x, y), circular_index)
- n_circular_index = int(numpy.floor((theta2 - theta1) / numpy.pi))
- theta2 = theta2 + ((circular_index - n_circular_index)) * numpy.pi
- return numpy.array((theta1, theta2))
-
-
-# alpha is in [0, 1] and is the weight to merge a and b.
-def alpha_blend(a, b, alpha):
- """Blends a and b.
-
- Args:
- alpha: double, Ratio. Needs to be in [0, 1] and is the weight to blend a
- and b.
- """
- return b * alpha + (1.0 - alpha) * a
-
-
-def normalize(v):
- """Normalize a vector while handling 0 length vectors."""
- norm = numpy.linalg.norm(v)
- if norm == 0:
- return v
- return v / norm
-
-
-# CI is circular index and allows selecting between all the stats that map
-# to the same x-y state (by giving them an integer index).
-# This will compute approximate first and second derivatives with respect
-# to path length.
-def to_theta_with_circular_index_and_derivs(x, y, dx, dy,
- circular_index_select):
- a = to_theta_with_circular_index(x, y, circular_index_select)
- b = to_theta_with_circular_index(x + dx * 0.0001, y + dy * 0.0001,
- circular_index_select)
- c = to_theta_with_circular_index(x - dx * 0.0001, y - dy * 0.0001,
- circular_index_select)
- d1 = normalize(b - a)
- d2 = normalize(c - a)
- accel = (d1 + d2) / numpy.linalg.norm(a - b)
- return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
-
-
-def to_theta_with_ci_and_derivs(p_prev, p, p_next, c_i_select):
- a = to_theta(p, c_i_select)
- b = to_theta(p_next, c_i_select)
- c = to_theta(p_prev, c_i_select)
- d1 = normalize(b - a)
- d2 = normalize(c - a)
- accel = (d1 + d2) / numpy.linalg.norm(a - b)
- return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
-
-
-# Generic subdivision algorithm.
-def subdivide(p1, p2, max_dist):
- dx = p2[0] - p1[0]
- dy = p2[1] - p1[1]
- dist = numpy.sqrt(dx**2 + dy**2)
- n = int(numpy.ceil(dist / max_dist))
- return [(alpha_blend(p1[0], p2[0],
- float(i) / n), alpha_blend(p1[1], p2[1],
- float(i) / n))
- for i in range(1, n + 1)]
-
-
-# convert from an xy space loop into a theta loop.
-# All segements are expected go from one "hyper-extension" boundary
-# to another, thus we must go backwards over the "loop" to get a loop in
-# x-y space.
-def to_theta_loop(lines, cross_point=-numpy.pi):
- out = []
- last_pt = lines[0]
- for n_pt in lines[1:]:
- for pt in subdivide(last_pt, n_pt, max_dist):
- out.append(to_theta(pt, 0, cross_point))
- last_pt = n_pt
- for n_pt in reversed(lines[:-1]):
- for pt in subdivide(last_pt, n_pt, max_dist):
- out.append(to_theta(pt, 1, cross_point))
- last_pt = n_pt
- return out
-
-
-# Convert a loop (list of line segments) into
-# The name incorrectly suggests that it is cyclic.
-def back_to_xy_loop(lines):
- out = []
- last_pt = lines[0]
- out.append(to_xy(last_pt[0], last_pt[1]))
- for n_pt in lines[1:]:
- for pt in subdivide(last_pt, n_pt, max_dist_theta):
- out.append(to_xy(pt[0], pt[1]))
- last_pt = n_pt
-
- return out
-
-
-# Segment in angle space.
-class AngleSegment:
-
- def __init__(self, start, end, name=None, alpha_unitizer=None, vmax=None):
- """Creates an angle segment.
-
- Args:
- start: (double, double), The start of the segment in theta1, theta2
- coordinates in radians
- end: (double, double), The end of the segment in theta1, theta2
- coordinates in radians
- """
- self.start = start
- self.end = end
- self.name = name
- self.alpha_unitizer = alpha_unitizer
- self.vmax = vmax
-
- def __repr__(self):
- return "AngleSegment(%s, %s)" % (repr(self.start), repr(self.end))
-
- def DrawTo(self, cr, theta_version):
- if theta_version:
- cr.move_to(self.start[0], self.start[1] + theta_end_circle_size)
- cr.arc(self.start[0], self.start[1], theta_end_circle_size, 0,
- 2.0 * numpy.pi)
- cr.move_to(self.end[0], self.end[1] + theta_end_circle_size)
- cr.arc(self.end[0], self.end[1], theta_end_circle_size, 0,
- 2.0 * numpy.pi)
- cr.move_to(self.start[0], self.start[1])
- cr.line_to(self.end[0], self.end[1])
- else:
- start_xy = to_xy(self.start[0], self.start[1])
- end_xy = to_xy(self.end[0], self.end[1])
- draw_lines(cr, back_to_xy_loop([self.start, self.end]))
- cr.move_to(start_xy[0] + xy_end_circle_size, start_xy[1])
- cr.arc(start_xy[0], start_xy[1], xy_end_circle_size, 0,
- 2.0 * numpy.pi)
- cr.move_to(end_xy[0] + xy_end_circle_size, end_xy[1])
- cr.arc(end_xy[0], end_xy[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
-
- def ToThetaPoints(self):
- dx = self.end[0] - self.start[0]
- dy = self.end[1] - self.start[1]
- mag = numpy.hypot(dx, dy)
- dx /= mag
- dy /= mag
-
- return [(self.start[0], self.start[1], dx, dy, 0.0, 0.0),
- (self.end[0], self.end[1], dx, dy, 0.0, 0.0)]
-
-
-class XYSegment:
- """Straight line in XY space."""
-
- def __init__(self, start, end, name=None, alpha_unitizer=None, vmax=None):
- """Creates an XY segment.
-
- Args:
- start: (double, double), The start of the segment in theta1, theta2
- coordinates in radians
- end: (double, double), The end of the segment in theta1, theta2
- coordinates in radians
- """
- self.start = start
- self.end = end
- self.name = name
- self.alpha_unitizer = alpha_unitizer
- self.vmax = vmax
-
- def __repr__(self):
- return "XYSegment(%s, %s)" % (repr(self.start), repr(self.end))
-
- def DrawTo(self, cr, theta_version):
- if theta_version:
- theta1, theta2 = self.start
- circular_index_select = int(
- numpy.floor((self.start[1] - self.start[0]) / numpy.pi))
- start = get_xy(self.start)
- end = get_xy(self.end)
-
- ln = [(start[0], start[1]), (end[0], end[1])]
- draw_lines(cr, [
- to_theta_with_circular_index(x, y, circular_index_select)
- for x, y in subdivide_xy(ln)
- ])
- cr.move_to(self.start[0] + theta_end_circle_size, self.start[1])
- cr.arc(self.start[0], self.start[1], theta_end_circle_size, 0,
- 2.0 * numpy.pi)
- cr.move_to(self.end[0] + theta_end_circle_size, self.end[1])
- cr.arc(self.end[0], self.end[1], theta_end_circle_size, 0,
- 2.0 * numpy.pi)
- else:
- start = get_xy(self.start)
- end = get_xy(self.end)
- cr.move_to(start[0], start[1])
- cr.line_to(end[0], end[1])
- cr.move_to(start[0] + xy_end_circle_size, start[1])
- cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
- cr.move_to(end[0] + xy_end_circle_size, end[1])
- cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
-
- def ToThetaPoints(self):
- """ Converts to points in theta space via to_theta_with_circular_index_and_derivs"""
- theta1, theta2 = self.start
- circular_index_select = int(
- numpy.floor((self.start[1] - self.start[0]) / numpy.pi))
- start = get_xy(self.start)
- end = get_xy(self.end)
-
- ln = [(start[0], start[1]), (end[0], end[1])]
-
- dx = end[0] - start[0]
- dy = end[1] - start[1]
- mag = numpy.hypot(dx, dy)
- dx /= mag
- dy /= mag
-
- return [
- to_theta_with_circular_index_and_derivs(x, y, dx, dy,
- circular_index_select)
- for x, y in subdivide_xy(ln, 0.01)
- ]
-
-
-def spline_eval(start, control1, control2, end, alpha):
- a = alpha_blend(start, control1, alpha)
- b = alpha_blend(control1, control2, alpha)
- c = alpha_blend(control2, end, alpha)
- return alpha_blend(alpha_blend(a, b, alpha), alpha_blend(b, c, alpha),
- alpha)
-
-
-def subdivide_spline(start, control1, control2, end):
- # TODO: pick N based on spline parameters? or otherwise change it to be more evenly spaced?
- n = 100
- for i in range(0, n + 1):
- yield i / float(n)
-
-
-class SplineSegment:
-
- def __init__(self,
- start,
- control1,
- control2,
- end,
- name=None,
- alpha_unitizer=None,
- vmax=None):
- self.start = start
- self.control1 = control1
- self.control2 = control2
- self.end = end
- self.name = name
- self.alpha_unitizer = alpha_unitizer
- self.vmax = vmax
-
- def __repr__(self):
- return "SplineSegment(%s, %s, %s, %s)" % (repr(
- self.start), repr(self.control1), repr(
- self.control2), repr(self.end))
-
- def DrawTo(self, cr, theta_version):
- if theta_version:
- c_i_select = get_circular_index(self.start)
- start = get_xy(self.start)
- control1 = get_xy(self.control1)
- control2 = get_xy(self.control2)
- end = get_xy(self.end)
-
- draw_lines(cr, [
- to_theta(spline_eval(start, control1, control2, end, alpha),
- c_i_select)
- for alpha in subdivide_spline(start, control1, control2, end)
- ])
- cr.move_to(self.start[0] + theta_end_circle_size, self.start[1])
- cr.arc(self.start[0], self.start[1], theta_end_circle_size, 0,
- 2.0 * numpy.pi)
- cr.move_to(self.end[0] + theta_end_circle_size, self.end[1])
- cr.arc(self.end[0], self.end[1], theta_end_circle_size, 0,
- 2.0 * numpy.pi)
- else:
- start = get_xy(self.start)
- control1 = get_xy(self.control1)
- control2 = get_xy(self.control2)
- end = get_xy(self.end)
-
- draw_lines(cr, [
- spline_eval(start, control1, control2, end, alpha)
- for alpha in subdivide_spline(start, control1, control2, end)
- ])
-
- cr.move_to(start[0] + xy_end_circle_size, start[1])
- cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
- cr.move_to(end[0] + xy_end_circle_size, end[1])
- cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
-
- def ToThetaPoints(self):
- t1, t2 = self.start
- c_i_select = get_circular_index(self.start)
- start = get_xy(self.start)
- control1 = get_xy(self.control1)
- control2 = get_xy(self.control2)
- end = get_xy(self.end)
-
- return [
- to_theta_with_ci_and_derivs(
- spline_eval(start, control1, control2, end, alpha - 0.00001),
- spline_eval(start, control1, control2, end, alpha),
- spline_eval(start, control1, control2, end, alpha + 0.00001),
- c_i_select)
- for alpha in subdivide_spline(start, control1, control2, end)
- ]
-
-
-def get_derivs(t_prev, t, t_next):
- c, a, b = t_prev, t, t_next
- d1 = normalize(b - a)
- d2 = normalize(c - a)
- accel = (d1 + d2) / numpy.linalg.norm(a - b)
- return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
-
-
-class ThetaSplineSegment:
-
- def __init__(self,
- start,
- control1,
- control2,
- end,
- name=None,
- alpha_unitizer=None,
- vmax=None):
- self.start = start
- self.control1 = control1
- self.control2 = control2
- self.end = end
- self.name = name
- self.alpha_unitizer = alpha_unitizer
- self.vmax = vmax
-
- def __repr__(self):
- return "ThetaSplineSegment(%s, %s, &s, %s)" % (repr(
- self.start), repr(self.control1), repr(
- self.control2), repr(self.end))
-
- def DrawTo(self, cr, theta_version):
- if (theta_version):
- draw_lines(cr, [
- spline_eval(self.start, self.control1, self.control2, self.end,
- alpha)
- for alpha in subdivide_spline(self.start, self.control1,
- self.control2, self.end)
- ])
- else:
- start = get_xy(self.start)
- end = get_xy(self.end)
-
- draw_lines(cr, [
- get_xy(
- spline_eval(self.start, self.control1, self.control2,
- self.end, alpha))
- for alpha in subdivide_spline(self.start, self.control1,
- self.control2, self.end)
- ])
-
- cr.move_to(start[0] + xy_end_circle_size, start[1])
- cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
- cr.move_to(end[0] + xy_end_circle_size, end[1])
- cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
-
- def ToThetaPoints(self):
- return [
- get_derivs(
- spline_eval(self.start, self.control1, self.control2, self.end,
- alpha - 0.00001),
- spline_eval(self.start, self.control1, self.control2, self.end,
- alpha),
- spline_eval(self.start, self.control1, self.control2, self.end,
- alpha + 0.00001))
- for alpha in subdivide_spline(self.start, self.control1,
- self.control2, self.end)
- ]
-
-
-tall_box_x = 0.411
-tall_box_y = 0.125
-
-short_box_x = 0.431
-short_box_y = 0.082
-
-ready_above_box = to_theta_with_circular_index(tall_box_x,
- tall_box_y + 0.08,
- circular_index=-1)
-tall_box_grab = to_theta_with_circular_index(tall_box_x,
- tall_box_y,
- circular_index=-1)
-short_box_grab = to_theta_with_circular_index(short_box_x,
- short_box_y,
- circular_index=-1)
-
-# TODO(austin): Drive the front/back off the same numbers a bit better.
-front_high_box = to_theta_with_circular_index(0.378, 2.46, circular_index=-1)
-front_middle3_box = to_theta_with_circular_index(0.700,
- 2.125,
- circular_index=-1.000000)
-front_middle2_box = to_theta_with_circular_index(0.700,
- 2.268,
- circular_index=-1)
-front_middle1_box = to_theta_with_circular_index(0.800,
- 1.915,
- circular_index=-1)
-front_low_box = to_theta_with_circular_index(0.87, 1.572, circular_index=-1)
-back_high_box = to_theta_with_circular_index(-0.75, 2.48, circular_index=0)
-back_middle2_box = to_theta_with_circular_index(-0.700, 2.27, circular_index=0)
-back_middle1_box = to_theta_with_circular_index(-0.800, 1.93, circular_index=0)
-back_low_box = to_theta_with_circular_index(-0.87, 1.64, circular_index=0)
-
-back_extra_low_box = to_theta_with_circular_index(-0.87,
- 1.52,
- circular_index=0)
-
-front_switch = to_theta_with_circular_index(0.88, 0.967, circular_index=-1)
-back_switch = to_theta_with_circular_index(-0.88, 0.967, circular_index=-2)
-
-neutral = to_theta_with_circular_index(0.0, 0.33, circular_index=-1)
-
-up = to_theta_with_circular_index(0.0, 2.547, circular_index=-1)
-
-front_switch_auto = to_theta_with_circular_index(0.750,
- 2.20,
- circular_index=-1.000000)
-
-duck = numpy.array([numpy.pi / 2.0 - 0.92, numpy.pi / 2.0 - 4.26])
-
-starting = numpy.array([numpy.pi / 2.0 - 0.593329, numpy.pi / 2.0 - 3.749631])
-vertical_starting = numpy.array([numpy.pi / 2.0, -numpy.pi / 2.0])
-
-self_hang = numpy.array([numpy.pi / 2.0 - 0.191611, numpy.pi / 2.0])
-partner_hang = numpy.array([numpy.pi / 2.0 - (-0.30), numpy.pi / 2.0])
-
-above_hang = numpy.array([numpy.pi / 2.0 - 0.14, numpy.pi / 2.0 - (-0.165)])
-below_hang = numpy.array([numpy.pi / 2.0 - 0.39, numpy.pi / 2.0 - (-0.517)])
-
-up_c1 = to_theta((0.63, 1.17), circular_index=-1)
-up_c2 = to_theta((0.65, 1.62), circular_index=-1)
-
-front_high_box_c1 = to_theta((0.63, 1.04), circular_index=-1)
-front_high_box_c2 = to_theta((0.50, 1.60), circular_index=-1)
-
-front_middle2_box_c1 = to_theta((0.41, 0.83), circular_index=-1)
-front_middle2_box_c2 = to_theta((0.52, 1.30), circular_index=-1)
-
-front_middle1_box_c1 = to_theta((0.34, 0.82), circular_index=-1)
-front_middle1_box_c2 = to_theta((0.48, 1.15), circular_index=-1)
-
-#c1: (1.421433, -1.070254)
-#c2: (1.434384, -1.057803
-ready_above_box_c1 = numpy.array([1.480802, -1.081218])
-ready_above_box_c2 = numpy.array([1.391449, -1.060331])
-
-front_switch_c1 = numpy.array([1.903841, -0.622351])
-front_switch_c2 = numpy.array([1.903841, -0.622351])
-
-
-sparse_front_points = [
- (front_high_box, "FrontHighBox"),
- (front_middle2_box, "FrontMiddle2Box"),
- (front_middle3_box, "FrontMiddle3Box"),
- (front_middle1_box, "FrontMiddle1Box"),
- (front_low_box, "FrontLowBox"),
- (front_switch, "FrontSwitch"),
-] # yapf: disable
-
-sparse_back_points = [
- (back_high_box, "BackHighBox"),
- (back_middle2_box, "BackMiddle2Box"),
- (back_middle1_box, "BackMiddle1Box"),
- (back_low_box, "BackLowBox"),
- (back_extra_low_box, "BackExtraLowBox"),
-] # yapf: disable
-
-def expand_points(points, max_distance):
- """Expands a list of points to be at most max_distance apart
-
- Generates the paths to connect the new points to the closest input points,
- and the paths connecting the points.
-
- Args:
- points, list of tuple of point, name, The points to start with and fill
- in.
- max_distance, float, The max distance between two points when expanding
- the graph.
-
- Return:
- points, edges
- """
- result_points = [points[0]]
- result_paths = []
- for point, name in points[1:]:
- previous_point = result_points[-1][0]
- previous_point_xy = get_xy(previous_point)
- circular_index = get_circular_index(previous_point)
-
- point_xy = get_xy(point)
- norm = numpy.linalg.norm(point_xy - previous_point_xy)
- num_points = int(numpy.ceil(norm / max_distance))
- last_iteration_point = previous_point
- for subindex in range(1, num_points):
- subpoint = to_theta(alpha_blend(previous_point_xy, point_xy,
- float(subindex) / num_points),
- circular_index=circular_index)
- result_points.append(
- (subpoint, '%s%dof%d' % (name, subindex, num_points)))
- result_paths.append(
- XYSegment(last_iteration_point, subpoint, vmax=6.0))
- if (last_iteration_point != previous_point).any():
- result_paths.append(XYSegment(previous_point, subpoint))
- if subindex == num_points - 1:
- result_paths.append(XYSegment(subpoint, point, vmax=6.0))
- else:
- result_paths.append(XYSegment(subpoint, point))
- last_iteration_point = subpoint
- result_points.append((point, name))
-
- return result_points, result_paths
-
-
-front_points, front_paths = expand_points(sparse_front_points, 0.06)
-back_points, back_paths = expand_points(sparse_back_points, 0.06)
-
-points = [(ready_above_box, "ReadyAboveBox"),
- (tall_box_grab, "TallBoxGrab"),
- (short_box_grab, "ShortBoxGrab"),
- (back_switch, "BackSwitch"),
- (neutral, "Neutral"),
- (up, "Up"),
- (above_hang, "AboveHang"),
- (below_hang, "BelowHang"),
- (self_hang, "SelfHang"),
- (partner_hang, "PartnerHang"),
- (front_switch_auto, "FrontSwitchAuto"),
- (starting, "Starting"),
- (duck, "Duck"),
- (vertical_starting, "VerticalStarting"),
-] + front_points + back_points # yapf: disable
-
-duck_c1 = numpy.array([1.337111, -1.721008])
-duck_c2 = numpy.array([1.283701, -1.795519])
-
-ready_to_up_c1 = numpy.array([1.792962, 0.198329])
-ready_to_up_c2 = numpy.array([1.792962, 0.198329])
-
-front_switch_auto_c1 = numpy.array([1.792857, -0.372768])
-front_switch_auto_c2 = numpy.array([1.861885, -0.273664])
-
-# We need to define critical points so we can create paths connecting them.
-# TODO(austin): Attach velocities to the slow ones.
-ready_to_back_low_c1 = numpy.array([2.524325, 0.046417])
-
-neutral_to_back_low_c1 = numpy.array([2.381942, -0.070220])
-
-tall_to_back_low_c1 = numpy.array([2.603918, 0.088298])
-tall_to_back_low_c2 = numpy.array([1.605624, 1.003434])
-
-tall_to_back_high_c2 = numpy.array([1.508610, 0.946147])
-
-# If true, only plot the first named segment
-isolate = False
-
-long_alpha_unitizer = numpy.matrix([[1.0 / 17.0, 0.0], [0.0, 1.0 / 17.0]])
-
-neutral_to_back_c1 = numpy.array([0.702527, -2.618276])
-neutral_to_back_c2 = numpy.array([0.526914, -3.109691])
-
-named_segments = [
- ThetaSplineSegment(neutral, neutral_to_back_c1, neutral_to_back_c2,
- back_switch, "BackSwitch"),
- ThetaSplineSegment(neutral,
- neutral_to_back_low_c1,
- tall_to_back_high_c2,
- back_high_box,
- "NeutralBoxToHigh",
- alpha_unitizer=long_alpha_unitizer),
- ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_high_c2,
- back_middle2_box, "NeutralBoxToMiddle2",
- long_alpha_unitizer),
- ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_low_c2,
- back_middle1_box, "NeutralBoxToMiddle1",
- long_alpha_unitizer),
- ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_low_c2,
- back_low_box, "NeutralBoxToLow", long_alpha_unitizer),
- ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
- tall_to_back_high_c2, back_high_box, "ReadyBoxToHigh",
- long_alpha_unitizer),
- ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
- tall_to_back_high_c2, back_middle2_box,
- "ReadyBoxToMiddle2", long_alpha_unitizer),
- ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
- tall_to_back_low_c2, back_middle1_box,
- "ReadyBoxToMiddle1", long_alpha_unitizer),
- ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
- tall_to_back_low_c2, back_low_box, "ReadyBoxToLow",
- long_alpha_unitizer),
- ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
- tall_to_back_high_c2, back_high_box, "ShortBoxToHigh",
- long_alpha_unitizer),
- ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
- tall_to_back_high_c2, back_middle2_box,
- "ShortBoxToMiddle2", long_alpha_unitizer),
- ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
- tall_to_back_low_c2, back_middle1_box,
- "ShortBoxToMiddle1", long_alpha_unitizer),
- ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
- tall_to_back_low_c2, back_low_box, "ShortBoxToLow",
- long_alpha_unitizer),
- ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1,
- tall_to_back_high_c2, back_high_box, "TallBoxToHigh",
- long_alpha_unitizer),
- ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1,
- tall_to_back_high_c2, back_middle2_box,
- "TallBoxToMiddle2", long_alpha_unitizer),
- ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_low_c2,
- back_middle1_box, "TallBoxToMiddle1",
- long_alpha_unitizer),
- ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_low_c2,
- back_low_box, "TallBoxToLow", long_alpha_unitizer),
- SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
- ready_above_box, "ReadyToNeutral"),
- XYSegment(ready_above_box, tall_box_grab, "ReadyToTallBox", vmax=6.0),
- XYSegment(ready_above_box, short_box_grab, "ReadyToShortBox", vmax=6.0),
- XYSegment(tall_box_grab, short_box_grab, "TallToShortBox", vmax=6.0),
- SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
- tall_box_grab, "TallToNeutral"),
- SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
- short_box_grab, "ShortToNeutral"),
- SplineSegment(neutral, up_c1, up_c2, up, "NeutralToUp"),
- SplineSegment(neutral, front_high_box_c1, front_high_box_c2,
- front_high_box, "NeutralToFrontHigh"),
- SplineSegment(neutral, front_middle2_box_c1, front_middle2_box_c2,
- front_middle2_box, "NeutralToFrontMiddle2"),
- SplineSegment(neutral, front_middle1_box_c1, front_middle1_box_c2,
- front_middle1_box, "NeutralToFrontMiddle1"),
-]
-
-unnamed_segments = [
- SplineSegment(neutral, front_switch_auto_c1, front_switch_auto_c2,
- front_switch_auto),
- SplineSegment(tall_box_grab, ready_to_up_c1, ready_to_up_c2, up),
- SplineSegment(short_box_grab, ready_to_up_c1, ready_to_up_c2, up),
- SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2, up),
- ThetaSplineSegment(duck, duck_c1, duck_c2, neutral),
- SplineSegment(neutral, front_switch_c1, front_switch_c2, front_switch),
- XYSegment(ready_above_box, front_low_box),
- XYSegment(ready_above_box, front_switch),
- XYSegment(ready_above_box, front_middle1_box),
- XYSegment(ready_above_box, front_middle2_box),
- XYSegment(ready_above_box, front_middle3_box),
- SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2,
- front_high_box),
- AngleSegment(starting, vertical_starting),
- AngleSegment(vertical_starting, neutral),
- XYSegment(neutral, front_low_box),
- XYSegment(up, front_high_box),
- XYSegment(up, front_middle2_box),
- XYSegment(front_middle3_box, up),
- XYSegment(front_middle3_box, front_high_box),
- XYSegment(front_middle3_box, front_middle2_box),
- XYSegment(front_middle3_box, front_middle1_box),
- XYSegment(up, front_middle1_box),
- XYSegment(up, front_low_box),
- XYSegment(front_high_box, front_middle2_box),
- XYSegment(front_high_box, front_middle1_box),
- XYSegment(front_high_box, front_low_box),
- XYSegment(front_middle2_box, front_middle1_box),
- XYSegment(front_middle2_box, front_low_box),
- XYSegment(front_middle1_box, front_low_box),
- XYSegment(front_switch, front_low_box),
- XYSegment(front_switch, up),
- XYSegment(front_switch, front_high_box),
- AngleSegment(up, back_high_box),
- AngleSegment(up, back_middle2_box),
- AngleSegment(up, back_middle1_box),
- AngleSegment(up, back_low_box),
- XYSegment(back_high_box, back_middle2_box),
- XYSegment(back_high_box, back_middle1_box),
- XYSegment(back_high_box, back_low_box),
- XYSegment(back_middle2_box, back_middle1_box),
- XYSegment(back_middle2_box, back_low_box),
- XYSegment(back_middle1_box, back_low_box),
- AngleSegment(up, above_hang),
- AngleSegment(above_hang, below_hang),
- AngleSegment(up, below_hang),
- AngleSegment(up, self_hang),
- AngleSegment(up, partner_hang),
-] + front_paths + back_paths
-
-segments = []
-if isolate:
- segments += named_segments[:isolate]
-else:
- segments += named_segments + unnamed_segments
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
new file mode 100644
index 0000000..da0ad4f
--- /dev/null
+++ b/y2023/control_loops/python/graph_paths.py
@@ -0,0 +1,26 @@
+import numpy
+
+from graph_tools import *
+
+neutral = to_theta_with_circular_index(-0.2, 0.33, circular_index=-1)
+zero = to_theta_with_circular_index(0.0, 0.0, circular_index=-1)
+
+neutral_to_cone_1 = to_theta_with_circular_index(0.0, 0.7, circular_index=-1)
+neutral_to_cone_2 = to_theta_with_circular_index(0.2, 0.5, circular_index=-1)
+cone_pos = to_theta_with_circular_index(1.0, 0.4, circular_index=-1)
+
+neutral_to_cone_perch_pos_1 = to_theta_with_circular_index(0.4,
+ 1.0,
+ circular_index=-1)
+neutral_to_cone_perch_pos_2 = to_theta_with_circular_index(0.7,
+ 1.5,
+ circular_index=-1)
+cone_perch_pos = to_theta_with_circular_index(1.0, 2.0, circular_index=-1)
+
+segments = [
+ ThetaSplineSegment(neutral, neutral_to_cone_1, neutral_to_cone_2, cone_pos,
+ "NeutralToCone"),
+ ThetaSplineSegment(neutral, neutral_to_cone_perch_pos_1,
+ neutral_to_cone_perch_pos_2, cone_perch_pos,
+ "NeutralToPerchedCone"),
+]
diff --git a/y2023/control_loops/python/graph_tools.py b/y2023/control_loops/python/graph_tools.py
new file mode 100644
index 0000000..3ddfb37
--- /dev/null
+++ b/y2023/control_loops/python/graph_tools.py
@@ -0,0 +1,533 @@
+import numpy
+import cairo
+
+from frc971.control_loops.python.basic_window import OverrideMatrix, identity
+
+# joint_center in x-y space.
+joint_center = (-0.299, 0.299)
+
+# Joint distances (l1 = "proximal", l2 = "distal")
+l1 = 46.25 * 0.0254
+l2 = 43.75 * 0.0254
+
+max_dist = 0.01
+max_dist_theta = numpy.pi / 64
+xy_end_circle_size = 0.01
+theta_end_circle_size = 0.07
+
+
+def px(cr):
+ return OverrideMatrix(cr, identity)
+
+
+# Convert from x-y coordinates to theta coordinates.
+# orientation is a bool. This orientation is circular_index mod 2.
+# where circular_index is the circular index, or the position in the
+# "hyperextension" zones. "cross_point" allows shifting the place where
+# it rounds the result so that it draws nicer (no other functional differences).
+def to_theta(pt, circular_index, cross_point=-numpy.pi):
+ orient = (circular_index % 2) == 0
+ x = pt[0]
+ y = pt[1]
+ x -= joint_center[0]
+ y -= joint_center[1]
+ l3 = numpy.hypot(x, y)
+ t3 = numpy.arctan2(y, x)
+ theta1 = numpy.arccos((l1**2 + l3**2 - l2**2) / (2 * l1 * l3))
+
+ if orient:
+ theta1 = -theta1
+ theta1 += t3
+ theta1 = (theta1 - cross_point) % (2 * numpy.pi) + cross_point
+ theta2 = numpy.arctan2(y - l1 * numpy.sin(theta1),
+ x - l1 * numpy.cos(theta1))
+ return numpy.array((theta1, theta2))
+
+
+# Simple trig to go back from theta1, theta2 to x-y
+def to_xy(theta1, theta2):
+ x = numpy.cos(theta1) * l1 + numpy.cos(theta2) * l2 + joint_center[0]
+ y = numpy.sin(theta1) * l1 + numpy.sin(theta2) * l2 + joint_center[1]
+ orient = ((theta2 - theta1) % (2.0 * numpy.pi)) < numpy.pi
+ return (x, y, orient)
+
+
+def get_circular_index(theta):
+ return int(numpy.floor((theta[1] - theta[0]) / numpy.pi))
+
+
+def get_xy(theta):
+ theta1 = theta[0]
+ theta2 = theta[1]
+ x = numpy.cos(theta1) * l1 + numpy.cos(theta2) * l2 + joint_center[0]
+ y = numpy.sin(theta1) * l1 + numpy.sin(theta2) * l2 + joint_center[1]
+ return numpy.array((x, y))
+
+
+# Subdivide in theta space.
+def subdivide_theta(lines):
+ out = []
+ last_pt = lines[0]
+ out.append(last_pt)
+ for n_pt in lines[1:]:
+ for pt in subdivide(last_pt, n_pt, max_dist_theta):
+ out.append(pt)
+ last_pt = n_pt
+
+ return out
+
+
+# subdivide in xy space.
+def subdivide_xy(lines, max_dist=max_dist):
+ out = []
+ last_pt = lines[0]
+ out.append(last_pt)
+ for n_pt in lines[1:]:
+ for pt in subdivide(last_pt, n_pt, max_dist):
+ out.append(pt)
+ last_pt = n_pt
+
+ return out
+
+
+def to_theta_with_ci(pt, circular_index):
+ return to_theta_with_circular_index(pt[0], pt[1], circular_index)
+
+
+# to_theta, but distinguishes between
+def to_theta_with_circular_index(x, y, circular_index):
+ theta1, theta2 = to_theta((x, y), circular_index)
+ n_circular_index = int(numpy.floor((theta2 - theta1) / numpy.pi))
+ theta2 = theta2 + ((circular_index - n_circular_index)) * numpy.pi
+ return numpy.array((theta1, theta2))
+
+
+# alpha is in [0, 1] and is the weight to merge a and b.
+def alpha_blend(a, b, alpha):
+ """Blends a and b.
+
+ Args:
+ alpha: double, Ratio. Needs to be in [0, 1] and is the weight to blend a
+ and b.
+ """
+ return b * alpha + (1.0 - alpha) * a
+
+
+def normalize(v):
+ """Normalize a vector while handling 0 length vectors."""
+ norm = numpy.linalg.norm(v)
+ if norm == 0:
+ return v
+ return v / norm
+
+
+# CI is circular index and allows selecting between all the stats that map
+# to the same x-y state (by giving them an integer index).
+# This will compute approximate first and second derivatives with respect
+# to path length.
+def to_theta_with_circular_index_and_derivs(x, y, dx, dy,
+ circular_index_select):
+ a = to_theta_with_circular_index(x, y, circular_index_select)
+ b = to_theta_with_circular_index(x + dx * 0.0001, y + dy * 0.0001,
+ circular_index_select)
+ c = to_theta_with_circular_index(x - dx * 0.0001, y - dy * 0.0001,
+ circular_index_select)
+ d1 = normalize(b - a)
+ d2 = normalize(c - a)
+ accel = (d1 + d2) / numpy.linalg.norm(a - b)
+ return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
+
+
+def to_theta_with_ci_and_derivs(p_prev, p, p_next, c_i_select):
+ a = to_theta(p, c_i_select)
+ b = to_theta(p_next, c_i_select)
+ c = to_theta(p_prev, c_i_select)
+ d1 = normalize(b - a)
+ d2 = normalize(c - a)
+ accel = (d1 + d2) / numpy.linalg.norm(a - b)
+ return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
+
+
+# Generic subdivision algorithm.
+def subdivide(p1, p2, max_dist):
+ dx = p2[0] - p1[0]
+ dy = p2[1] - p1[1]
+ dist = numpy.sqrt(dx**2 + dy**2)
+ n = int(numpy.ceil(dist / max_dist))
+ return [(alpha_blend(p1[0], p2[0],
+ float(i) / n), alpha_blend(p1[1], p2[1],
+ float(i) / n))
+ for i in range(1, n + 1)]
+
+
+# convert from an xy space loop into a theta loop.
+# All segements are expected go from one "hyper-extension" boundary
+# to another, thus we must go backwards over the "loop" to get a loop in
+# x-y space.
+def to_theta_loop(lines, cross_point=-numpy.pi):
+ out = []
+ last_pt = lines[0]
+ for n_pt in lines[1:]:
+ for pt in subdivide(last_pt, n_pt, max_dist):
+ out.append(to_theta(pt, 0, cross_point))
+ last_pt = n_pt
+ for n_pt in reversed(lines[:-1]):
+ for pt in subdivide(last_pt, n_pt, max_dist):
+ out.append(to_theta(pt, 1, cross_point))
+ last_pt = n_pt
+ return out
+
+
+# Convert a loop (list of line segments) into
+# The name incorrectly suggests that it is cyclic.
+def back_to_xy_loop(lines):
+ out = []
+ last_pt = lines[0]
+ out.append(to_xy(last_pt[0], last_pt[1]))
+ for n_pt in lines[1:]:
+ for pt in subdivide(last_pt, n_pt, max_dist_theta):
+ out.append(to_xy(pt[0], pt[1]))
+ last_pt = n_pt
+
+ return out
+
+
+def spline_eval(start, control1, control2, end, alpha):
+ a = alpha_blend(start, control1, alpha)
+ b = alpha_blend(control1, control2, alpha)
+ c = alpha_blend(control2, end, alpha)
+ return alpha_blend(alpha_blend(a, b, alpha), alpha_blend(b, c, alpha),
+ alpha)
+
+
+def subdivide_spline(start, control1, control2, end):
+ # TODO: pick N based on spline parameters? or otherwise change it to be more evenly spaced?
+ n = 100
+ for i in range(0, n + 1):
+ yield i / float(n)
+
+
+def get_derivs(t_prev, t, t_next):
+ c, a, b = t_prev, t, t_next
+ d1 = normalize(b - a)
+ d2 = normalize(c - a)
+ accel = (d1 + d2) / numpy.linalg.norm(a - b)
+ return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
+
+
+# Draw lines to cr + stroke.
+def draw_lines(cr, lines):
+ cr.move_to(lines[0][0], lines[0][1])
+ for pt in lines[1:]:
+ cr.line_to(pt[0], pt[1])
+ with px(cr):
+ cr.stroke()
+
+
+# Segment in angle space.
+class AngleSegment:
+
+ def __init__(self, start, end, name=None, alpha_unitizer=None, vmax=None):
+ """Creates an angle segment.
+
+ Args:
+ start: (double, double), The start of the segment in theta1, theta2
+ coordinates in radians
+ end: (double, double), The end of the segment in theta1, theta2
+ coordinates in radians
+ """
+ self.start = start
+ self.end = end
+ self.name = name
+ self.alpha_unitizer = alpha_unitizer
+ self.vmax = vmax
+
+ def __repr__(self):
+ return "AngleSegment(%s, %s)" % (repr(self.start), repr(self.end))
+
+ def DrawTo(self, cr, theta_version):
+ if theta_version:
+ cr.move_to(self.start[0], self.start[1] + theta_end_circle_size)
+ cr.arc(self.start[0], self.start[1], theta_end_circle_size, 0,
+ 2.0 * numpy.pi)
+ cr.move_to(self.end[0], self.end[1] + theta_end_circle_size)
+ cr.arc(self.end[0], self.end[1], theta_end_circle_size, 0,
+ 2.0 * numpy.pi)
+ cr.move_to(self.start[0], self.start[1])
+ cr.line_to(self.end[0], self.end[1])
+ else:
+ start_xy = to_xy(self.start[0], self.start[1])
+ end_xy = to_xy(self.end[0], self.end[1])
+ draw_lines(cr, back_to_xy_loop([self.start, self.end]))
+ cr.move_to(start_xy[0] + xy_end_circle_size, start_xy[1])
+ cr.arc(start_xy[0], start_xy[1], xy_end_circle_size, 0,
+ 2.0 * numpy.pi)
+ cr.move_to(end_xy[0] + xy_end_circle_size, end_xy[1])
+ cr.arc(end_xy[0], end_xy[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+
+ def ToThetaPoints(self):
+ dx = self.end[0] - self.start[0]
+ dy = self.end[1] - self.start[1]
+ mag = numpy.hypot(dx, dy)
+ dx /= mag
+ dy /= mag
+
+ return [(self.start[0], self.start[1], dx, dy, 0.0, 0.0),
+ (self.end[0], self.end[1], dx, dy, 0.0, 0.0)]
+
+
+class XYSegment:
+ """Straight line in XY space."""
+
+ def __init__(self, start, end, name=None, alpha_unitizer=None, vmax=None):
+ """Creates an XY segment.
+
+ Args:
+ start: (double, double), The start of the segment in theta1, theta2
+ coordinates in radians
+ end: (double, double), The end of the segment in theta1, theta2
+ coordinates in radians
+ """
+ self.start = start
+ self.end = end
+ self.name = name
+ self.alpha_unitizer = alpha_unitizer
+ self.vmax = vmax
+
+ def __repr__(self):
+ return "XYSegment(%s, %s)" % (repr(self.start), repr(self.end))
+
+ def DrawTo(self, cr, theta_version):
+ if theta_version:
+ theta1, theta2 = self.start
+ circular_index_select = int(
+ numpy.floor((self.start[1] - self.start[0]) / numpy.pi))
+ start = get_xy(self.start)
+ end = get_xy(self.end)
+
+ ln = [(start[0], start[1]), (end[0], end[1])]
+ draw_lines(cr, [
+ to_theta_with_circular_index(x, y, circular_index_select)
+ for x, y in subdivide_xy(ln)
+ ])
+ cr.move_to(self.start[0] + theta_end_circle_size, self.start[1])
+ cr.arc(self.start[0], self.start[1], theta_end_circle_size, 0,
+ 2.0 * numpy.pi)
+ cr.move_to(self.end[0] + theta_end_circle_size, self.end[1])
+ cr.arc(self.end[0], self.end[1], theta_end_circle_size, 0,
+ 2.0 * numpy.pi)
+ else:
+ start = get_xy(self.start)
+ end = get_xy(self.end)
+ cr.move_to(start[0], start[1])
+ cr.line_to(end[0], end[1])
+ cr.move_to(start[0] + xy_end_circle_size, start[1])
+ cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+ cr.move_to(end[0] + xy_end_circle_size, end[1])
+ cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+
+ def ToThetaPoints(self):
+ """ Converts to points in theta space via to_theta_with_circular_index_and_derivs"""
+ theta1, theta2 = self.start
+ circular_index_select = int(
+ numpy.floor((self.start[1] - self.start[0]) / numpy.pi))
+ start = get_xy(self.start)
+ end = get_xy(self.end)
+
+ ln = [(start[0], start[1]), (end[0], end[1])]
+
+ dx = end[0] - start[0]
+ dy = end[1] - start[1]
+ mag = numpy.hypot(dx, dy)
+ dx /= mag
+ dy /= mag
+
+ return [
+ to_theta_with_circular_index_and_derivs(x, y, dx, dy,
+ circular_index_select)
+ for x, y in subdivide_xy(ln, 0.01)
+ ]
+
+
+class SplineSegment:
+
+ def __init__(self,
+ start,
+ control1,
+ control2,
+ end,
+ name=None,
+ alpha_unitizer=None,
+ vmax=None):
+ self.start = start
+ self.control1 = control1
+ self.control2 = control2
+ self.end = end
+ self.name = name
+ self.alpha_unitizer = alpha_unitizer
+ self.vmax = vmax
+
+ def __repr__(self):
+ return "SplineSegment(%s, %s, %s, %s)" % (repr(
+ self.start), repr(self.control1), repr(
+ self.control2), repr(self.end))
+
+ def DrawTo(self, cr, theta_version):
+ if theta_version:
+ c_i_select = get_circular_index(self.start)
+ start = get_xy(self.start)
+ control1 = get_xy(self.control1)
+ control2 = get_xy(self.control2)
+ end = get_xy(self.end)
+
+ draw_lines(cr, [
+ to_theta(spline_eval(start, control1, control2, end, alpha),
+ c_i_select)
+ for alpha in subdivide_spline(start, control1, control2, end)
+ ])
+ cr.move_to(self.start[0] + theta_end_circle_size, self.start[1])
+ cr.arc(self.start[0], self.start[1], theta_end_circle_size, 0,
+ 2.0 * numpy.pi)
+ cr.move_to(self.end[0] + theta_end_circle_size, self.end[1])
+ cr.arc(self.end[0], self.end[1], theta_end_circle_size, 0,
+ 2.0 * numpy.pi)
+ else:
+ start = get_xy(self.start)
+ control1 = get_xy(self.control1)
+ control2 = get_xy(self.control2)
+ end = get_xy(self.end)
+
+ draw_lines(cr, [
+ spline_eval(start, control1, control2, end, alpha)
+ for alpha in subdivide_spline(start, control1, control2, end)
+ ])
+
+ cr.move_to(start[0] + xy_end_circle_size, start[1])
+ cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+ cr.move_to(end[0] + xy_end_circle_size, end[1])
+ cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+
+ def ToThetaPoints(self):
+ t1, t2 = self.start
+ c_i_select = get_circular_index(self.start)
+ start = get_xy(self.start)
+ control1 = get_xy(self.control1)
+ control2 = get_xy(self.control2)
+ end = get_xy(self.end)
+
+ return [
+ to_theta_with_ci_and_derivs(
+ spline_eval(start, control1, control2, end, alpha - 0.00001),
+ spline_eval(start, control1, control2, end, alpha),
+ spline_eval(start, control1, control2, end, alpha + 0.00001),
+ c_i_select)
+ for alpha in subdivide_spline(start, control1, control2, end)
+ ]
+
+
+class ThetaSplineSegment:
+
+ def __init__(self,
+ start,
+ control1,
+ control2,
+ end,
+ name=None,
+ alpha_unitizer=None,
+ vmax=None):
+ self.start = start
+ self.control1 = control1
+ self.control2 = control2
+ self.end = end
+ self.name = name
+ self.alpha_unitizer = alpha_unitizer
+ self.vmax = vmax
+
+ def __repr__(self):
+ return "ThetaSplineSegment(%s, %s, &s, %s)" % (repr(
+ self.start), repr(self.control1), repr(
+ self.control2), repr(self.end))
+
+ def DrawTo(self, cr, theta_version):
+ if (theta_version):
+ draw_lines(cr, [
+ spline_eval(self.start, self.control1, self.control2, self.end,
+ alpha)
+ for alpha in subdivide_spline(self.start, self.control1,
+ self.control2, self.end)
+ ])
+ else:
+ start = get_xy(self.start)
+ end = get_xy(self.end)
+
+ draw_lines(cr, [
+ get_xy(
+ spline_eval(self.start, self.control1, self.control2,
+ self.end, alpha))
+ for alpha in subdivide_spline(self.start, self.control1,
+ self.control2, self.end)
+ ])
+
+ cr.move_to(start[0] + xy_end_circle_size, start[1])
+ cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+ cr.move_to(end[0] + xy_end_circle_size, end[1])
+ cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+
+ def ToThetaPoints(self):
+ return [
+ get_derivs(
+ spline_eval(self.start, self.control1, self.control2, self.end,
+ alpha - 0.00001),
+ spline_eval(self.start, self.control1, self.control2, self.end,
+ alpha),
+ spline_eval(self.start, self.control1, self.control2, self.end,
+ alpha + 0.00001))
+ for alpha in subdivide_spline(self.start, self.control1,
+ self.control2, self.end)
+ ]
+
+
+def expand_points(points, max_distance):
+ """Expands a list of points to be at most max_distance apart
+
+ Generates the paths to connect the new points to the closest input points,
+ and the paths connecting the points.
+
+ Args:
+ points, list of tuple of point, name, The points to start with and fill
+ in.
+ max_distance, float, The max distance between two points when expanding
+ the graph.
+
+ Return:
+ points, edges
+ """
+ result_points = [points[0]]
+ result_paths = []
+ for point, name in points[1:]:
+ previous_point = result_points[-1][0]
+ previous_point_xy = get_xy(previous_point)
+ circular_index = get_circular_index(previous_point)
+
+ point_xy = get_xy(point)
+ norm = numpy.linalg.norm(point_xy - previous_point_xy)
+ num_points = int(numpy.ceil(norm / max_distance))
+ last_iteration_point = previous_point
+ for subindex in range(1, num_points):
+ subpoint = to_theta(alpha_blend(previous_point_xy, point_xy,
+ float(subindex) / num_points),
+ circular_index=circular_index)
+ result_points.append(
+ (subpoint, '%s%dof%d' % (name, subindex, num_points)))
+ result_paths.append(
+ XYSegment(last_iteration_point, subpoint, vmax=6.0))
+ if (last_iteration_point != previous_point).any():
+ result_paths.append(XYSegment(previous_point, subpoint))
+ if subindex == num_points - 1:
+ result_paths.append(XYSegment(subpoint, point, vmax=6.0))
+ else:
+ result_paths.append(XYSegment(subpoint, point))
+ last_iteration_point = subpoint
+ result_points.append((point, name))
+
+ return result_points, result_paths
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index d20a247..cbfd3f6 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -9,15 +9,19 @@
namespace y2023 {
namespace vision {
+namespace chrono = std::chrono;
+
AprilRoboticsDetector::AprilRoboticsDetector(aos::EventLoop *event_loop,
std::string_view channel_name)
: calibration_data_(CalibrationData()),
ftrace_(),
- image_callback_(event_loop, channel_name,
- [&](cv::Mat image_color_mat,
- const aos::monotonic_clock::time_point eof) {
- HandleImage(image_color_mat, eof);
- }),
+ image_callback_(
+ event_loop, channel_name,
+ [&](cv::Mat image_color_mat,
+ const aos::monotonic_clock::time_point eof) {
+ HandleImage(image_color_mat, eof);
+ },
+ chrono::milliseconds(5)),
target_map_sender_(
event_loop->MakeSender<frc971::vision::TargetMap>("/camera")) {
tag_family_ = tag16h5_create();
@@ -151,8 +155,8 @@
aos::monotonic_clock::now();
VLOG(1) << "Took "
- << std::chrono::duration<double>(after_pose_estimation -
- before_pose_estimation)
+ << chrono::duration<double>(after_pose_estimation -
+ before_pose_estimation)
.count()
<< " seconds for pose estimation";
}
@@ -164,8 +168,7 @@
timeprofile_display(tag_detector_->tp);
- VLOG(1) << "Took "
- << std::chrono::duration<double>(end_time - start_time).count()
+ VLOG(1) << "Took " << chrono::duration<double>(end_time - start_time).count()
<< " seconds to detect overall";
return results;
diff --git a/y2023/vision/viewer.cc b/y2023/vision/viewer.cc
index 08def5b..68495b1 100644
--- a/y2023/vision/viewer.cc
+++ b/y2023/vision/viewer.cc
@@ -14,6 +14,8 @@
DEFINE_string(capture, "",
"If set, capture a single image and save it to this filename.");
+DEFINE_int32(rate, 100, "Time in milliseconds to wait between images");
+
namespace frc971 {
namespace vision {
namespace {
@@ -78,7 +80,7 @@
event_loop.Exit();
};
},
- ::std::chrono::milliseconds(100));
+ ::std::chrono::milliseconds(FLAGS_rate));
event_loop.Run();