sot-talos-balance requirements

Hallo! currently, in sot-talos-balance, we are using parametric-curves in our class NdTrajectoryGenerator.

I write here which kinds of trajectories we are actively using, how we are using them, what requirements and improvements I would like to see, and also the other trajectories which are included in this class but which we are not using.

As I do not know the package curves, please forgive me if I ask for obvious requirements or for requirements which are already satisfied.

First requirement: it should be possible to refer to a curve in a generic way without regard to its actual type, by proper subclassing, i.e. there should be an abstract curve class which all other curves inherit from.

Second requirement: trajectories should be available at least up to the acceleration level (with a possible exception for text-based trajectories, more on that below).

Third requirement: all trajectories should be executable for any amount of time, including infinity.

Actively using

• Constant: we should be able to set it at will, at any time

• MinimumJerk: this trajectory allows to execute a rest-to-rest trajectory (0 initial and final velocity and acceleration) from point A to point B in a given amonut of time

  $T$
  by minimizing the time integral of the squared jerk, i.e:
  $$\min\int_0^T \left(\frac{{\mathrm d}^3 x}{{\mathrm d}t^3}\right)^2 {\mathrm d}t$$

  This amounts to a fifth-order polynomial with appropriate coefficients. So, normally, if generic polynomials are already implemented, then a separate class may not be strictly needed, but this is a common use case and it might be nice to have it (not to mention that a dedicated implementation might behave better numerically). Details on the implementation can be found in class MinimumJerk

• Sinusoid: the current parametric-curves implementation InfiniteSinusoid allows to specify an "initial point" (which is take to correspond to a stationary point, either minimum or maximum indifferently) a "final point" (the other stationary point, maximum or minimum), and a "trajectory time" (the time it takes to go from the initial point to the final point, i.e. half the period). Then the trajectory with start from the initial point, go to the initial point, and continues indefinitely. In other words it implements

  x(t) = x_0 + \frac{x_f-x_0}{2}\left(1-\cos\left(\frac{\pi}{T}t\right)\right)

  This is equivalent to allowing to specify amplitude, period, and offset with respect to zero, but not phase, because the trajectory always starts at a stationary point. If you want to parameterize the trajectory in a different way (i.e. amplitude, period, phase, offset) that is fine with me because the calculations from one to the other are straightforward, just make sure that the current trajectories are still achievable by some proper selection of the parameters

• Reading from text files. For this, I do not need fancy serialization of curves in parametric form. This can come later. All I need is the ability to read from data stored in plain text format, one row per time instant, each row corresponding to a vector of predetermined length and possibly its derivatives, each element being separated by spaces. So, for instance, the CoM is a three-element vector, and it should stored together with its velocity and acceleration like this:

  1.0 2.0 3.0 4.0 5.0 6.0 7.0 8.0 9.0
  1.1 2.1 3.1 4.1 5.1 6.1 7.1 8.1 9.1

  where [1.0 2.0 3.0] is the position, [4.0 5.0 6.0] is the velocity, and [7.0 8.0 9.0] is the acceleration at instant 0, [1.1 2.1 3.1], [4.1 5.1 6.1] and [7.1 8.1 9.1] are position, velocity and acceleration at instant 1, ecc. No time specification is needed in the file, it is assumed that the object reading it knows the frequency. When the file is over, the trajectory should steadily keep issuing its final value.

  Notice there are several issues in the current parametric-curves implementation in TextFile, which are the main reason why I am pushing for change. Therefore, I would really appreciate to see this issues addressed in curves, if they are not already solved. Here I list them, from most to least important (in my opinion). Unfortunately, I think the most important ones are also the most difficult to address, so good work!

  1. Size. This is possibly the most limiting one. Currently, TextFile reads the file by storing its values into a buffer, but the size of this buffer is limited to 1,000,000 double. If the file is bigger that that, an error message is issued and unwanted behavior is produced (I think an empty matrix is returned). This is severely limiting and should be addressed. This might be trickier than it seems. A buffer is certainly necessary, because we cannot afford to do a disk access each time we need a new value, i.e. at each time instant. But simply increasing the buffer will just push the limit forward, not remove it. On the other hand we should avoid having huge buffers in order not to fill the memory. Maybe some work with threads will be needed
  2. File loading. Strictly related to the previous point. The current file loading process is blocking and can generate failure on the robot with RT constrains. We could use a separate thread to read the data. See this issue comment
  3. The current implementation expects the file to contain position, velocity and acceleration. If velocity and acceleration are not needed, one is obliged to provide them anyways, filling the file with dummy values, thereby greatly increasing the size of the file for nothing. It should be possible to specify we only care about values up to a certain time derivative. This might help alleviate the first point, because less values will be stored. What happens if, for instance, velocity is asked but the file only contains position, do as you want, it can cause a crash, or return zero, or again try a numerical differentiation, I don't care
  4. The current implementation is buggy when it comes to the final instant: the buffer is read out of bounds and it returns garbage. In sot-talos-balance, I had to implement the workaround of asking the time instant just before the last one in order to avoid this bug
  5. The current implementation expects the position, velocity and acceleration vector to be all of the same size. This means that, for instance, we cannot include a quaternion for the orientation followed by a 3D vector for the angular velocity: we are stuck to Euler angles. It would be nice to allow for different sizes, at least between position and higher derivatives
  6. The current implementation seems not to be robust to slight variations of the format. For instance, if a line ends with a trailing white space, it will crash. Some flexibility will probably make out life easier

Currently unused

Below you find a list of curves which are available in NdTrajectoryGenerator, but which we are not actively using. I'll list them without comments

• LinearChirp (a linear-chirp trajectory, that is a sinusoidal trajectory with frequency being a linear function of time)
• InfiniteConstAcc (an infinite trajectory with piece-wise constant acceleration)
• Spline

changed the description

mentioned in issue sot-talos-balance#94

mentioned in issue #7

mentioned in issue #8 (closed)

Hi, I opened individual issues for each new class of curves required.

First requirement: it should be possible to refer to a curve in a generic way without regard to its actual type, by proper subclassing, i.e. there should be an abstract curve class which all other curves inherit from.

Second requirement: trajectories should be available at least up to the acceleration level (with a possible exception for text-based trajectories, more on that below).

This is ok in the current version.

Third requirement: all trajectories should be executable for any amount of time, including infinity.

This is not possible with the current version, I do not understand exactly this requirement. All the curves are defined on a specific time interval (except infiniteSinusoid and constant), trying to evaluate the curve outside of this interval will raise an error. What it is current behaviour of parametric-curves when you evaluate a curve outside of it's bounds ?

changed the description

Third requirement: all trajectories should be executable for any amount of time, including infinity.

This is not possible with the current version, I do not understand exactly this requirement. All the curves are defined on a specific time interval (except infiniteSinusoid and constant), trying to evaluate the curve outside of this interval will raise an error. What it is current behaviour of parametric-curves when you evaluate a curve outside of it's bounds ?

I don't see the need of limiting the curves to a time interval. If you have a polynomial, its value can be calculated for any time instant. It other cases, you may still return the final value, setting all derivatives to zero.

As for the current behavior in parametric-curves:

Actively used in sot-talos-balance

• Constant: unlimited
• InfiniteSinusoid: unlimited
• MinimumJerk does not perform any check and simply applies the formula. I think this is the correct behavior.
• TextFile does not perform any check and it reads out of the buffer, returning garbage. This is clearly bad. A check should be performed. I think the best would be that, if the file is read out of bounds, then the final value is returned.

Not actively used in sot-talos-balance, but present in NdTrajectoryGenerator

• Spline literally returns random garbage. It constructs an output, it leaves it uninitialized, and returns it.

  const point_t dummy;
  return dummy;

  Again, wrong. I think the final value of the final subspline should be returned.

• InfiniteConstAcc: unlimited

• LinearChirp: unlimited

Mostly what I want to avoid is that if I am wrong by a dt I get garbage in return, or an arbitrary value such as zero. If a sensible value cannot be returned, then an exception should be raised and the whole program should crash.

But I would rather have some leniency and return sensible values, such as the ones explained above. I think it is reasonable: if you work with floating-point values you may be slightly off, and this should not be a problem. That's what happened to me in sot-talos-balance. NdTrajectoryGenerator was trying to access TextFile after the maximum time had passed, and I got garbage. Of course, had the program crashed, I would have seen the error earlier, but still I would have had to implement the same ugly workaround to fix it. Instead, returning the final value would have been a sensible choice.

Actually I was wrong, by default you can evaluate the curves at any times. But there is a template parameter Safe, if True you cannot evaluate the curve outside of an interval given to the constructor without raising an error.

So the only issue here is that in the python bindings this parameter Safe is always set to True and there is no bindings to change this.

Additionally, we should check that when Safe = False no classes of curves return undefined value when called outside of definition range.

I close this Issue, all the remaining requirements have been added as separate issues.

closed

mentioned in issue #13

See my comment on Safe https://gepgitlab.laas.fr/loco-3d/curves/issues/13 . Evaluating curves such as Bezier curves on infinite intervals does not make sense at all.

Additionally, it is easy to define an infinite interval if this is actually required