New Primitives Integration

[dennisss]

Trajectory code in #142

@dmz38 @abramowitzK see https://github.com/dennisss/tansa/blob/new/include/tansa/trajectory.h for the prototypes.

Below are some options/parameters that would need to be in the CSV:

Types:

• Ellipse
  • circle parameters but separate x and y radius
• Any trajectory rotated or translated
  • Angles rotationX, rotationY, rotationZ
  • OffsetX, offsetY, offsetZ
• Spiral: origin, radius, start time and angle, end time and angle
• Arcs:
  • n points and n times

[dmz38]

RE: Trajectories from our good friend Dennis

[Rotating and translating occur] both at the same time. There is a TransformedTrajectory class that will take in both at once. setting one or the other to identity / zero vector will only do one.

I've found that rotating things usually takes them underground, so translation is very useful. especially because rotating a circle at (0,0,1) origin will make the origin at (0,1,0) potentially, so we should heed rotating before translating to the right origin

[dennisss]

Also, this file contains examples of everything I simulated a week ago (https://github.com/dennisss/tansa/blob/master/src/custom.cpp#L1)

The easiest way to specify rotations would be to multiple together Eigen AxisAngle objects (https://github.com/dennisss/tansa/blob/master/src/custom.cpp#L166). In that line, it rotates pi/2 radians around the y-axis

[dennisss]

Another small note: if making arcs, times must be exact integers, otherwise CGAL has a problem solving them.

[abramowitzK]

Added support for ellipse, spiral and arc in CSV. Don't have Stationary points yet in CSV format so will have to adjust for that later. Should you be able to specify velocity and acceleration at each point as well for arcs? @dennisss

[dennisss]

@abramowitzK Cool! We could specify those, but I think that wouldn't be as useful for users are those are harder to visualize. Maybe accelerations as that would specify the orientation at that point, but that's something we'd need to ask Mike if he'd want to use

[abramowitzK]

@dennisss Okay sounds good. Can add that later. Working on transformed trajectories. A little tricky in csv.

[abramowitzK]

Ellipse [x] Spiral [x] Transformed Trajectories [x] Arc[ ]

Still having trouble with arcs, CGAL doesn't like the parameters I'm putting in, will have to experiment further.

Also, all of these need to be tested way more thoroughly. I'm going to write a large test suite when i have time.

[dennisss]

@abramowitzK , keeping it to all round integer values has worked for me. It's been very stubborn in only operating in the rational number space, still looking for a way around that with using a harder to get library.

[abramowitzK]

@dennisss Do the points have to be integers as well as the times?

[dennisss]

@abramowitzK , maybe. Have never tried with non-integer points 🤔

[abramowitzK]

@dennisss I'm triggering a very descriptive assert of ( j <= i ) somewhere in CGAL but my points are never integers because they are feet being converted to meters. I feel like the assert must be something else wrong though

Edit:

Expr: j <= i File: /usr/include/CGAL/QP_models.h Line: 809

Gonna go check it out.

[abramowitzK]

minsnap.cpp Crashes in this call in one of the CGAL routines 209: bool success = qp_solve(Q, A, b, rels, x, cost);

// Copying values into program
for(int i = 0; i < Q.rows(); i++) {
    for(int j = 0; j < Q.cols(); j++) {
        qp.set_d(i, j, Q(i, j)); <-- When j is 1 and i is 0 this crashes
    }
}

[dennisss]

Ok. I think I found the issue. Writing a patch... Also, I checked and we should be good on non-integer points

[abramowitzK]

@dennisss awesome, thanks!

[dennisss]

Try https://github.com/dennisss/tansa/commit/b55002c6928f4fd7fd3fc4267c0523a10f3d59d5 available in new2 branch

[abramowitzK]

@dennisss still crashes. it tries to call set_d with i = 0 and j = 1 still

[abramowitzK]

the matrix i have here is 10x10, is that what it is supposed to be?

[abramowitzK]

what about this?

// Copying values into program
for(int i = 0; i < Q.rows(); i++) {
    for(int j = 0; j <= i; j++) {
        qp.set_d(i, j, Q(i, j));
    }
}

Now i get this :) progress :thinking: ?

CGAL warning: check violation! Expression : is_integer(d) File : /usr/include/CGAL/GMP/Gmpz_type.h Line : 102 Explanation: Gmpz constructed from non-integer double value Refer to the bug-reporting instructions at http://www.cgal.org/bug_report.html CGAL warning: check violation! Expression : is_integer(d) File : /usr/include/CGAL/GMP/Gmpz_type.h Line : 102 Explanation: Gmpz constructed from non-integer double value Refer to the bug-reporting instructions at http://www.cgal.org/bug_report.html CGAL warning: check violation! Expression : is_integer(d) File : /usr/include/CGAL/GMP/Gmpz_type.h Line : 102 Explanation: Gmpz constructed from non-integer double value Refer to the bug-reporting instructions at http://www.cgal.org/bug_report.html

[dennisss]

Yeah, that should work. I am confused why we seem to have different versions of CGAL. You can try flipping the CGAL_USE_GMP #define at the top of that file.

[dennisss]

Yeah, using the MP_Float does seem to make CGAL play nicer. It seem to allow non-integer times but is much slower.

[abramowitzK]

Doesn't complain about integers but still fails to find solution. Here are my params { {{2,2,2}} <--Points not calling ConstrainedPoint::Stationary on them though will try that {{4,4,4}} } {0, 5} <-- times

[dennisss]

I'd think that for non-stationary endpoints, it's too under constrained and the best solution is at -infinity.

[abramowitzK]

I'll keep trying. Might just have bad values or something because it seems like it should work now. but is_infeasable() (line 376 minsnap.cpp) is always true. Might just sleep on it and figure out tomorrow/tuesday

[abramowitzK]

Think I found issue. Not with your code with mine. Will report back when I fix

[abramowitzK]

Okay now it doesn't complain about integers but just makes an infeasible solution (╯°□°）╯︵ ┻━┻ Bedtime

Edit: Fixed it. I should have gone to bed a while ago didn't realize I messed up the data file when I was debugging. It works now but GMP still doesn't like non integer points I also kept that change i made in minsnap.cpp for loop

[abramowitzK]

Did some testing. GMP does NOT work with non integer points. It gives completely different (incorrect) results when turned off.

[dennisss]

ok. for now let's get the rest of the primitives merged, so that we have something ready for Mike with CSV