trajectory/Reviews

Package proposal

Proposal for a package that takes as input a WaypointTrajWithLimits message - a set of positions, (velocities, accelerations, times) and parameterizes them using different types of spline representations. The input message includes specifications of joint limits and whether the joint wraps around.

Why do we need this package

Here's a simple example of why we need this package. Suppose you have two joints and a planner/decision maker gives you a path for the two joints of this form:

Note how jagged the path is. Further, its possible that this path does not obey velocity and acceleration limits for this particular joint. This package provides functionality to convert this path into a time parameterized trajectory that obeys velocity and acceleration limits. E.g., let the maximum velocities for joints 1 and 2 be (0.5,0.25) and the maximum accelerations be (0.25,0.5). A trajectory that obeys these limits and yet goes through the positions specified in the input is given in the figures below. Note that the velocities and accelerations for each joint do not exceed the specified limits.

API Proposal

The API is simple. A base class contains two main functions:

   bool set(const manipulation_msgs::WaypointTrajWithLimits& trajectory_in,
   manipulation_msgs::SplineTraj &spline)

This function call parameterizes the trajectory and returns the spline representation of the trajectory. The spline representation consists of n-1 segments where 'n' is the number of points in the trajectory.
- trajectory_in is the input trajectory that consists of a set of positions for a number of joints. It also contains information about the joint limits. Encoding the limits inside the message helps since then the trajectory package does not have to be urdf aware. Also, there is the likelihood that this will find applications in parameterizing paths for things that don't exist in the urdf (e.g. 2D position of the base, etc.)
- spline is the output quintic spline. Not all the coefficients may be filled in (they are initialized to zero in that case).
```
   bool sample(const manipulation_msgs::SplineTraj& spline, 
   const std::vector<double> &times, 
   manipulation_msgs::WaypointTraj& traj_out)
```
- spline is the input spline that contains the spline representation of the trajectory.
- times is a vector containing the times at which the trajectory should be sampled. The time=0.0 represent the time at the beginning of the whole trajectory.

In addition, there are a few helper functions.

```
   bool getTotalTime(const manipulation_msgs::SplineTraj &spline, double &t)
```
- Returns the total time for the trajectory - the trajectory gets rescaled so this is a nice utility function to have if you want to call sample with the correct set of times for the total trajectory.
```
   bool write(const manipulation_msgs::SplineTraj &spline, 
   conststd::vector<double> times, std::string filename)
```
- Again, a useful utility function to write out trajectories into a file.
```
   bool writeSpline(const manipulation_msgs::SplineTraj &spline, 
   std::string filename)
```
- A useful utility function to write out the spline parameters to file.
There are also a couple of protected virtual classes that the derived classes can implement:
```
   virtual bool sample(const manipulation_msgs::SplineTrajSegment &spline, 
   const double& t, manipulation_msgs::Waypoint &point_out)
```
- This function takes as input a spline segment and a segment time and fills in the waypoint corresponding to that time. The implementation in the base class uses the expansion of the quintic polynomial to fill in the relevant quantities but derived classes are free to define their own versions of sample.
```
   virtual bool parameterize(const manipulation_msgs::WaypointTrajWithLimits&
   trajectory_in, manipulation_msgs::SplineTraj& spline) = 0
```
- A pure virtual function that has to be implemented by the derived class. This function does the actual work of parameterizing according to the particular implementation of the derived class.

Question / concerns / comments

Stu

My main issue is that this is not a generic "trajectory" package. It's doing a specific thing: it's resampling a trajectory into a list of splines, and then sampling from those splines. Let's call this something like "trajectory_resampler" and avoid trying to create a generic trajectory library right now.

  What should a generic trajectory library be trying to do?

Additionally, it's difficult to tell what it handles as input. I expect it to take in some combination of positions, velocities, accelerations, and times, but looking at the code I'm pretty sure it doesn't take in every combination. Documenting precisely what it takes in would be a good start.

  It can take in some combinations of those. The particular combination it takes in depends on the implementation - linear splines will just connect positions together, and so on

I also have a few lower-level suggestions:

Several of the functions have really generic names: set and parameterize are the main offenders.
```
  Agreed. These could be merged.
```
None of the functions depend on any state within the class. They should probably be static or moved outside of a class. (Vijay agrees)
```
  How would you deal with multiple implementations then? In particular, how would this work with the pluginlib architecture?
```

In conclusion, I don't think this should become a public API. It will be useful internally for certain nodes, but it needs a few more iterations before I would consider the API polished and ready for external consumption.

  I disagree, I think this in combination with the spline_smoother node should be put out soon. We already have a need for this both in Peter's work and the upcoming work we will be doing with move_arm

Vijay

For set(), the input is a WaypointTrajWithLimits. Do you just ignore the velocity and acceleration terms? If that's the case, then maybe the input should be a message that doesn't have velocities and accelerations.
- ```
    Some implementations will use the velocity and acceleration terms while others won't. That depends on the degree of the polynomial you end up using for parameterization.
```
Do you use the time fields in WaypointTraj? It should be clear whether you are or not, since the function will look very different depending on which it is.
```
   Yes, see the times in the example figures. They are rescaled to conform to the joint limits in the messages.
```
set() should probably be virtual
- set() also looks a lot like parameterize. Why do you need both?
- ```
   Agreed, they should be the same
```
Are you sure that you want to use double for times everywhere? It might easier to interface with the controller if they're all ros::Time
- ```
   I would rather not use ros::Time for efficiency considerations
```

Looking at the data flow, it is WaypointTrajWithLimits -> SplineTraj -> WaypointTraj. This naming seems a little confusing.

    The interface can be redefined. In particular, !WaypointTrajWithLimits may need to be redefined so it uses !WaypointTraj directly instead of duplicating it

I'm not convinced that sample should take a vector of times as input. I think the nominal use case is going to be sampling at only a single time.

   The planning stack often requires the entire sampling of a trajectory so that the planner/collision checker can know what the controller will be doing

In WaypointTraj.msg, I would rename names and limits to joint_names and joint_limits

   I am not so sure, I have used the trajectory package for things other than joints - 2D position of the base although these could be considered joints.

I don't think sample() should be virtual. Since the SplineTrajSegment message has clearly defines how it should be sampled, the only sampler that makes sense is the quintic polynomial sampler.
- ```
    Not true. In particular, if we implement other things like B-splines, this is no longer true. In fact, it might be best to move away from the quintic spline representation.
```
- I'm not convinced that the sampling and the parameterizing code need to live in the same class. They're two very independent pieces of code.
```
    Possibly. This would also depend on how we end up representing the intermediate stage in between sampling and parameterizing. 
```

Old Package proposal

Proposal for a trajectory package that does general n-DOF trajectory interpolation. This package is needed to specify trajectories for the arm/base and interpolate between points in the trajectory.

Features

Trajectories will be specified as a std::vector of points of type TPoint. TPoint is a struct which essentially defines a n DOF std::vector of positions and an n DOF std::vector of velocities with a timestamp.
The TPoints defined in the trajectory are via-points specified by the user. User input can be in two forms - as a trajectory with timestamps or just a path with no timestamps.
A separate TCoeff struct contains the coefficients of the polynomial used to interpolate between two TPoint s of the trajectory. This struct consists of the following:
- degree of the polynomial being used - linear, quadratic, cubic, etc.
- dimension of the vector - same as the dimension of the trajectory
- std::vector<std::vector<double>> set of coefficents
The interface to the class will be the following:
- instantiation - Trajectory(int nDof);
- setting the trajectory
  - setTrajectory(const std::vector<TPoint> &tp - set using a vector of trajectory points
  - int setTrajectory(const std::vector<double> &p, const std::vector<double> &time, int numPoints) - set using a vector of values and a vector of times along with number of points being set
  - int setTrajectory(const std::vector<double> &p, int numPoints) - set using only a vector of values
- setting the interpolation type
  - linear (for now)
  - quadratic
  - cubic, etc.
- sampling
  - int sample(TPoint &tp, double time) - returns the value at that time according to the interpolation scheme
  - sampling based either on timestamps specified by the user or auto-calculation using max rates, accns specified by the user
Other proposed features
- interpolation in SE(3) i.e. for coordinate frames
- choice of interpolation decides continuity/smoothness etc.
- Timestamps specified faster than max accn constraints
  - if flag (autocalc_timing_) true - limits/constraints always imposed
- sanity checks on trajectories - before outputting sample
  - isnan
  - isinf
- Need to return velocities and accelerations as well as positions using the sample functions

Question / concerns / comments

Enter your thoughts on the design and any questions / concerns you have here. Please sign your name.

Vijay

What aspects of this package should be realtime safe? (Traj Coefficient calculation, traj coefficient evaluation, etc)
- if you pre-allocate a trajectory, this is realtime safe
- not realtime safe for adding points
- sampling safe in realtime
Should there be the possibility of specifying periodic trajectories?
- eg sawtooth can be specified using corner points as waypoints
Do we expect controllers to own a trajectory interpolators? Or, do we want to send preprocessed trajectories to controllers?
- Yes, but the controller has to be careful using it - check errors to make sure they are not large

Package review meeting notes

Separate class for end-effector trajectories?
int sample(std::vector<TPoint> &tp, double start_time, double end_time, double dT) - sample between start and end times using a time interval of dT between samples
int sample(std::vector<TPoint> &tp, double dT) - sample entire trajectory
retrieve the way points without sampling the trajectory - getWayPoints
insert points - resample the trajectory
be able to insert waiting times?
set using velocities as well - set the maximum acceleration
General state based constraints - use a user-defined callback function?

Create new package review

Enter the date in the appropriate box to generate a template for an API or code review meeting. Check the QAProcess page for more information.