Generated paths don't look like examples

[jdmulder03]

I cant seem to get similar results to the examples. When running the below code I generate the following csv. https://drive.google.com/file/d/1jdLjwmF8Lp2PzlMDfUTpHnOuJLM29De4/view?usp=sharing

#include "csvLogger.hpp"
#include "squiggles.hpp"

const double MAX_VEL     = 2.0; // in meters per second
const double MAX_ACCEL   = 3.0; // in meters per second per second
const double MAX_JERK    = 6.0; // in meters per second per second per second
const double ROBOT_WIDTH = 0.4; // in meters

squiggles::Constraints constraints = squiggles::Constraints(MAX_VEL, MAX_ACCEL, MAX_JERK);
squiggles::SplineGenerator generator = squiggles::SplineGenerator(
  constraints,
  std::make_shared<squiggles::TankModel>(ROBOT_WIDTH, constraints));

std::vector<squiggles::ProfilePoint> path = generator.generate({
  squiggles::Pose(0.0, 0.0, 1.0),
  squiggles::Pose(0.0, 2.0, 1.0)});

int main() {
  auto logs = Logs();
  logs.add(path);
  logs.writeToCSV("generationTest2.csv");
  return 0;
}

the shape looks similar to the example but looking at the scales its more compressed than the example

[baylessj]

Thanks for bringing this to my attention, my testing wasn't sufficient to catch this error.

I'm pretty sure the root of the issue is the K_DEFAULT_VEL constant, linked below: https://github.com/baylessj/robotsquiggles/blob/dbbae1f77e0a94cf3db311ac2ffc3b2c66fad225/main/include/spline.hpp#L94

My testing with visualizing paths had involved non-zero starting and ending velocities so this constant was not applied. The polynomials that form the path need some sort of a "dummy velocity" to make the shape of the path. This constant determines that shape -- the higher the constant, the curvier the path.

A K_DEFAULT_VEL value closer to 1.0 seems to be getting me better results in my testing. Can you try that value out with your robot and see if it works better?

Ideally I'd like to find a solution where this constant doesn't carry so much weight, either by allowing it to be an optional parameter to the generation of a path or running the path through an additional round of fit checking to get a nice looking path.

[jdmulder03]

1.0 seemed like too much, a value of 0.75 to 0.4 seems to work great for my vex sized paths.

1.0

0.6

Definitely seems like you'd want different parameters for different sizes of paths/robot dynamics. If the solution is an optional parameter during generation I think I could PR that.

[baylessj]

I've done some more testing with that constant today and I'm not sure that the current implementation with it is all that great. You're correct that a constant of 0.6 is the best fit for the path you showed above, and it's not a bad option for other paths, but it's not the best for everything.

My perspective is that the goal for the generated paths should be to minimize spikes in the curvature of the path -- we don't want a sharp turn at the beginning and end like in your original post or a sharp turn in the middle like with the 1.0 constant. I'm thinking that a more robust solution would be to modify the dummy start/end velocities in the gen_raw_path step and consider the curvature in the acceptance criteria for a good path before starting parameterization.

I'm going to tinker with that idea over the next couple of days and see if that will be a good permanent solution for the path shape issues. I'll also add the optional parameter for the default vel constant while I'm at it since that will probably involve a decent number of changes, but I really appreciate you volunteering to put in a PR for that!

[baylessj]

@Dr-Danimal #39 should hopefully resolve the issue you're seeing. Can you try out the code in that PR and add a review to it?

[jdmulder03]

@baylessj shape looks good now. For some paths the new version fails, generating a acceleration beyond constraints, where the last version would successfully generate a path.

[baylessj]

Thanks for the review! Can you share an example set of waypoints that are failing under the new version? I'd like to get those working and prevent any breaking changes.

[jdmulder03]

x = 0, y = 0, theta = 0 x = 0.5, y = 0.5, theta = 0

[jdmulder03]

@baylessj bump

[baylessj]

The path you listed generates fine on the plotting utility in the squiggles repo, could the issue be coming from a difference in our constraints? I have {max_vel: 2.0, max_accel: 4.0, max_jerk: 10.0} and no specified maximum curvature. What constraints are you testing with?

[jdmulder03]

I'm using {max_vel: 1.5, max_accel: 0.5, max_jerk: 6} and for the old version a dummy initial velocity of 0.4. For this specific path the gradient descent optimizer fails to find a path that satisfies the acceleration constraint.

[baylessj]

Thanks for specifying the constraints, that was the missing piece in my testing. I found that there was an error with the exit conditions for the gradient descent loop that caused the path to no longer be generated. Your test path now works in my testing, hopefully it's working well for you too.