Codecov Report

Merging #461 (7f01403) into develop (a76d819) will increase coverage by 0.18%.
The diff coverage is 67.21%.

@@             Coverage Diff             @@
##           develop     #461      +/-   ##
===========================================
+ Coverage    88.72%   88.91%   +0.18%     
===========================================
  Files          140      139       -1     
  Lines         5880     5841      -39     
===========================================
- Hits          5217     5193      -24     
+ Misses         663      648      -15     

It's looking like the easiest way to get that test to compile would be to open a PR in your Okapilib Docker image to bump to Ubuntu 20.04 and GCC9 to get the filesystem support. GCC8 has issues with LCOV that prevent it from being a valid option but I've got GCC9 working well for the Squiggles CI. Does that work for you @Octogonapus ?

It's looking like the easiest way to get that test to compile would be to open a PR in your Okapilib Docker image to bump to Ubuntu 20.04 and GCC9 to get the filesystem support. GCC8 has issues with LCOV that prevent it from being a valid option but I've got GCC9 working well for the Squiggles CI. Does that work for you @Octogonapus ?

Not really, I would like to keep the same toolchain. Just use a different solution instead of <filesystem>.

Sounds good, switched to using the C API to get the path. It's a bit clunkier but should work.

I just remembered that I'll probably want to pin the squiggles version at a specific number rather than using the latest from the main branch. So long as there aren't further changes that need to happen with the Squiggles library for this PR then I can create a 1.0.0 release for it and pin the dependency there

Looks good. Just need to run some tests on a real robot now. It would be great if we could see videos of these tests. Not sure if you can do this @baylessj. I can't. Maybe @theol0403 or @dcieslak19973?

I could physically test; do you have an example path in mind? Would you want to demonstrate that paths can have negative wheel velocities? Adding documentation about how to pair this with the OkapiLib velocity PID controller would be nice.

I cannot physically test this, @dcieslak19973 's help would be great.

I would like to get verification that a path with negative velocities works fine. The exact parameters to make that happen would depend on the track width of your robot, but something where the robot is moving horizontally should work. The points I used to generate the path with negative velocities in the Squiggles README were {0, 0, 10_deg}->{0, 2_m, 20_deg} and a track width of 0.4_m. A similar set of points should get you a nice path; you can also verify the output path with the python visualizer in the Squiggles repo if you'd like.

Besides that, I would recommend testing with any existing paths you have using the current version of Okapilib (if any) or just making some mock auton movements.

The testing should work fine without integrating the AMPC with a velocity PID but I can look into writing a tutorial for that. There's similar content in the Squiggles docs but it might be nice to have a similar tutorial in the Okapi docs.

It's certainly possible I've built your branch incorrectly, but I am getting:

prosv5 make
Compiled src/main.cpp [WARNINGS]
In file included from ./include/okapi/api.hpp:51:0,
                 from ./include/main.h:42,
                 from ./include/globals.h:1,
                 from ./include/auton.h:1,
                 from src/main.cpp:1:
./include/okapi/api/control/async/asyncLinearMotionProfileController.hpp:18:10: fatal error: squiggles.hpp: No such file or directory
 #include "squiggles.hpp"
          ^~~~~~~~~~~~~~~
compilation terminated.
mv: cannot stat '.d/main.Td': No such file or directory
make: [common.mk:271: bin/main.cpp.o] Error 1 (ignored)
Capturing metadata for PROS Editor...

To get to this point, I changed the version in the Makefile to 4.2.0-dev, got the .zip artifact into my test project, ran pros c fetch okapilib@4.2.0-dev.zip on it and then prosv5 c apply okapilib --force-apply --no-download

It's certainly possible I've built your branch incorrectly, but I am getting:

prosv5 make
Compiled src/main.cpp [WARNINGS]
In file included from ./include/okapi/api.hpp:51:0,
                 from ./include/main.h:42,
                 from ./include/globals.h:1,
                 from ./include/auton.h:1,
                 from src/main.cpp:1:
./include/okapi/api/control/async/asyncLinearMotionProfileController.hpp:18:10: fatal error: squiggles.hpp: No such file or directory
 #include "squiggles.hpp"
          ^~~~~~~~~~~~~~~
compilation terminated.
mv: cannot stat '.d/main.Td': No such file or directory
make: [common.mk:271: bin/main.cpp.o] Error 1 (ignored)
Capturing metadata for PROS Editor...

To get to this point, I changed the version in the Makefile to 4.2.0-dev, got the .zip artifact into my test project, ran pros c fetch okapilib@4.2.0-dev.zip on it and then prosv5 c apply okapilib --force-apply --no-download

I'm a bit confused by the error message you've shown, I haven't seen it before. I'll write out the steps I would recommend taking to incorporate my fork into your PROS project; hopefully this will be a good checklist to follow.

• Clone my fork of okapilib (baylessj/okapilib)
• switch to the feature/squiggles branch
• in the okapilib root directory, run:
  • make
  • make template
  • prosv5 c f okapilib@*.zip
• Then, in your project directory, run:
  • prosv5 c a ../OkapiLib/okapilib@*.zip
  • make

This builds on my machine and hopefully should for yours as well. If it does not, there's a chance that you could be missing a dependency or have a different compiler version. If this fails, please list your gcc version (arm-none-eabi-gcc --version) and CMake version (cmake --version)

@baylessj - I agree, the co-ordinate transformations are not pleasant.

I'll try what you propose above later tonight, namely:

squiggles::Pose{point.x.convert(meter),
                      (-1 * point.y).convert(meter),
                      (-1 * point.theta).convert(radian)});

I may also change my test program to to switch to CARTESIAN .withOdometry configuration in the chassis builder and see where that goes.

It's possible that the twoEncoderOdometry.cpp transformation is incorrect. Perhaps @theol0403 or @Octogonapus could weigh in on it.

@baylessj

I switched my test program to use OkapiLib CARTESIAN and changed my path-points passed into the motionprofiler to correspond with those. The different co-ordinate conventions are making my head spin. In AsyncMotionProfileController::generatePath, using this snippet:

  std::vector<squiggles::Pose> points;
  points.reserve(iwaypoints.size());
  for (auto &point : iwaypoints) {
    points.push_back(
      squiggles::Pose{point.x.convert(meter), point.y.convert(meter), (90_deg - point.theta).convert(radian)});
  }

Produces the desired movement. I did try squiggles::Pose{point.x.convert(meter), -1.0 * point.y.convert(meter), -1.0 * point.theta.convert(radian)}); as well as a couple other permutations which would generally produce counter-clockwise movement when clockwise movement was expected.

During the movements, I had a separate thread output the OkapiLib (Cartesian) co-ordinates/heading which matched expectation. My hope is that this work now will allow for an easier time should one decide to try to implement a pure-pursuit path follower down the road.

The change listed above does make the unit tests on the AsyncMotionProfileControllerTest.FollowPath* fail. I did not physically test reversed and mirrored paths.

@dcieslak19973 The transform you listed above differs slightly from the one that you posted before, you had x and y flipped originally but not in the second example. I think your first transformation should work. For reference I'm going to compare a vector in each coordinate system:

^ 60 / ^ ^
| / | |
1 | x | X (Okapi) | Y (Squiggles)
| | |
------------> ---------> ---------->
2 Y X

Yaw is measured from X to Y in both frames.

Using the above point as an example, we should have {1, 2, 60} in the Okapilib FRAME_TRANSFORMATION frame. This should become {2, 1, 30} in the Squiggles form.

This looks to me like your first transformation would work then. Can you try that for an end point in each quadrant and verify that it works? (Like {2, 1, 30}, {-2, 1, -30}, {2, -1, 120}, {-2, -1, -120}, for example)

@baylessj - that's correct. The first transform which flips x and y works assuming coordinates are sent in using OkapiLib.FRAME_TRANSFORMATION (which appears to be the the default for the chassis builder), while the second transform works assuming OkapiLib.CARTESIAN.

I think the "correct" solution would be for the motion profiler to handle both cases. However, I think that could be done as a follow on PR, as I think it would potentially change the method signature (to accept the new parameter defaulted to FRAME_TRANSFORMATION), or to infer it based on the chassis controller. But the motion profilers seem to take chassisModel and not chassisController in their constructors (which I'm not quite sure what the rationale there is).

TL;DR - in the short term it's probably sufficient to implement:

squiggles::Pose{point.y.convert(meter),
                      point.x.convert(meter),
                      (90_deg - point.theta).convert(radian)});

With the suggestion that the comments make clear that the waypoints passed in are expected to be OkapiLib.FRAME_TRANSFORMATION format.

That makes sense to me, thanks for the clarification. I'll implement the transformation that you have listed above and specify the frame transformation in the docs. This will match with the current behavior like I'm intending; the goal is to not create any breaking changes here.

Nice. Thanks @baylessj

@theol0403 / @Octogonapus - I'm a +1

@theol0403 / @Octogonapus - bump

So the last details have been worked out and tested on a real robot then?

Thank you for your work on this @dcieslak19973 and @baylessj

Yes; physically tested on a real robot

@dcieslak19973 Update: @baylessj and I talked offline and we are going to wait on this PR until @baylessj finishes some changes in Squiggles.

The issues blocking the PR mentioned above have been resolved. I'd like the code to be retested but then it should be good to go.

@dcieslak19973 Would you be able to repeat the test cases you had run before but with the latest changes to this PR?

@baylessj - I should be able to take a look at it sometime in the next week

I've confirmed that it works as it did before. I did throw one potential edge case at it which looks to generate a runtime exception (I think it was an impossible path exception):

  profileController->generatePath({
      {0_ft, 0_ft, 0_deg},  // Profile starting position, this will normally be (0, 0, 0)
      {0_ft, 0_ft, 90_deg}}, // Point turn - this may be unfair
      "A" // Profile name
    );
  printf("Setting Target A\n");
  profileController->setTarget("A");
  printf("Set Target A\n");
  profileController->waitUntilSettled();
  printf("Settled\n");

Thank you for testing this! I'm not surprised that the point turn fails, although now that you bring it up it might be a nice option to support point turns in the future. I'll open an issue for that on the squiggles repo and look into supporting it in the future.