Get the state (or context) and output trajectories out of the Simulator

[RussTedrake]

I'm sure this is already on your radar.. but I'd love to discuss (and understand your estimated timeline).

In the matlab implementation, i found it very useful to define a Trajectory class which could be evaluated at any valid time. For continuous signals, the particular integration scheme used by the simulator could determine what type of trajectory interpolation was returned. (E.g. Euler might return a first-order hold whereas RK4 might return a cubic polynomial).

As an aside, the trajectory class also implemented the system interface, so could be easily wired up e.g. as an input to drive another simulation.

[sherm1]

I like the TrajectorySystem concept but it wasn't on our radar. For Simulator-generated trajectories, how do you envision the trajectory getting loaded in? Does it just accumulate the trajectory in memory and then get reused immediately? Or should this require serialization so that the trajectory can be reloaded at a later date?

[RussTedrake]

serialization would be great for the future, but there will already be use cases if it is simply in memory. the motivation for my request was so that I could make a plot of the simulation results. For another immediate example, trajectory optimization might output a trajectory object, which would be used immediately in simulation.

[edrumwri]

Could you comment further, Russ? Which plotting/processing tools will be used- MATLAB, Octave, matplotlib, some novel C++ tool? What kind of trajectory data- kinematics, state, controls?

On Mon, Oct 10, 2016 at 5:48 PM, Russ Tedrake notifications@github.com wrote:

serialization would be great for the future, but there will already be use cases if it is simply in memory. the motivation for my request was so that I could make a plot of the simulation results. For another immediate example, trajectory optimization might output a trajectory object, which would be used immediately in simulation.

— You are receiving this because you were assigned. Reply to this email directly, view it on GitHub https://github.com/RobotLocomotion/drake/issues/3734#issuecomment-252783325, or mute the thread https://github.com/notifications/unsubscribe-auth/ACHwz5Hm_EL0ZSfxDV0ep7ej-Kp52Uqnks5qytz0gaJpZM4KSC1v .

[RussTedrake]

see #3647

[Lucy-tri]

I’ll be working on a Trajectory parent class in C++. Both Trajectory Optimization and Inverse Kinematics are going to use the new Trajectory class for their output.

[sherm1]

I’ll be working on a Trajectory parent class in C++.

Awesome! Can I please kibitz on the spec? I'd like to make sure it can work for simulator trajectories too.

[Lucy-tri]

@sherm1 of course. All input welcome. Maybe I'll make a proposal for the API and then all can discuss.

[Lucy-tri]

Just talked this through with Sam. I will create a new interface (abstract class) Trajectory, and start by making PiecewisePolynomial inherit from that. This will be the first type of trajectory to be implemented as a subclass of Trajectory (more to come). I’ll start with that, then perhaps modify the IK stuff to use the new Trajectory class. Trajectory will define at least the following for the API, as they are defined in PiecewisePolynomial.h:

value(t) // eval() in Matlab derivative(derivative_order)

Russ said that C++ Trajectory should not inherit from System, (as it does in Matlab).
Feedback welcome.

[sherm1]

@Lucy-tri, I'm not sure what you mean by having PiecewisePolynomial inherit from Trajectory. PiecewisePolynomial already exists independently (and is not a trajectory). Do you mean that you would make a PiecewisePolynomialTrajectory class that inherits from Trajectory?

[sammy-tri]

@sherm1 The thought was that PiecewisePolynomial is already being used as a trajectory directly in some C++ code (e.g. TimeVaryingPolynomialSource), and a subset of its API seems to already meet (or be very close to) some of the properties of a Trajectory API, such as returning it's state at a specific time (value()). I'm not sure if the derivatives() interface is actually an analog for the MATLAB interface to get the derivatives of a trajectory.

We could, of course, create a PiecewisePolynomialTrajectory which takes a PiecewisePolynomal and is basically a shim between two (nearly?) identical APIs. Making the Trajectory interface a subset of the PiecewisePolynomial API seemed like less code... but we might have been too short-sighted there and the wrapper is actually better. Or maybe we/I should not have started treating PiecewisePolynomial as a de facto trajectory implementation in the first place. The PiecewisePolynomialTrajectory wrapper might also be cleaner as an example implementation of the API since the exposed interface would be much smaller than PiecewisePolynomial?

[sherm1]

@sammy-tri, thanks -- yes, I think it would be cleaner to have a PPTrajectory that is-a Trajectory and has-a PP internally. I'm also thinking that the Trajectory base API may fatten up with more functionality that isn't related to PP when we start loading it with simulator output. That's going to have to include discrete state variables (that have no derivatives), information needed to guide interpolation, possibly second derivatives and event info.

[Lucy-tri]

@sherm1 I think that makes more sense. Trajectory will have only the value() function (for now), and PiecewisePolynomialTrajectory will be a subclass which has a PiecewisePolynomial as a member.

[Lucy-tri]

~~Some details on my first move: I’m moving ReconstructInputTrajectory() and ReconstructStateTrajectory() out of direct_trajectory_optimization.cc: I need to remove the mention of a PP function out of that file. I’ll make GetTimeVector() (and it’s 2 pals) public. I’ll move ReconstructInputTrajectory() (and state…) to PiecewisePolynomial.~~ Looks like this was misguided. @hongkai-dai gave me some feedback.

[Lucy-tri]

I think the “output” of a PiecewisePolynomialTrajectory on a pendulum would be the result of calling DircolTrajectoryOptimization’s ReconstructInputTrajectory() & ReconstructStateTrajectory(). Is that right? I’ll change the reconstruct calls to return a PiecewisePolynomialTrajectory, a subclass of Trajectory, which should fullfill the requirement of this issue. @hongkai-dai @naveenoid or anyone else: Can you tell me if I understand this correctly?

[hongkai-dai]

@Lucy-tri , sorry for being late in the discussion. I agree with what you said, that in DirectTrajectoryOptimization class, there should be two methods ReconstructInputTrajectory and ReconstructStateTrajectory, each method returns a PiecewisePolynomialTrajectory object.

[Lucy-tri]

I've created a PR for a new C++ class Trajectory and a child PiecewisePolynomialTrajectory. You might want to wait for Sam' feature review before diving in, but here it is: https://reviewable.io/reviews/RobotLocomotion/drake/3856#-

[RussTedrake]

FYI - implementing the time-varying LQR solutions (it needs the integrator to compute the solution to the differential riccati equation, which then get’s looked up at control time) is currently blocked on this.

[Lucy-tri]

Part 2 of the new Trajectory class is in https://reviewable.io/reviews/RobotLocomotion/drake/4100#-

[RussTedrake]

Note that simulink provides a few options: https://www.mathworks.com/help/simulink/export-runtime-information.html

Blocks which logs signals (which I just implemented) is one of them. Exporting the data directly from the simulator is another. I figured it wouldn’t hurt to have the SignalLogger block, as everyone has suggested, which gets me unstuck for now.

[Lucy-tri]

@RussTedrake Is there more work here to do? If so, can you specify? (Or can we close this issue?). This relates to Asana task "Trajectory Optimization" (https://app.asana.com/0/194654295955291/194654295955291)

[RussTedrake]

Definitely not resolved IMHO. The ask was that I could call "StepTo" on the simulator and get back (or somehow have access to) a trajectory object representing the state and/or outputs, with the interpolation scheme being set properly by the integrator.

[sammy-tri]

@RussTedrake I was brainstorming the other day while talking to @Lucy-tri as to what the results of this would look like. I was thinking something like running a simulation with SignalLogger connected to the output, and then passing the SignalLogger to a class which looks something like: ` class ReplayTrajectory : public Trajectory {

ReplayTrajectory(const SignalLogger& logged_data, enum InterpolationScheme interpolation_scheme); ` (where I'm not precisely sure how one would get the interpolation scheme from the integrator)... then the result of that can be fed into TrajectorySource (or whatever used you wanted). Is that roughly the direction you had in mind?

[sherm1]

I'm not precisely sure how one would get the interpolation scheme from the integrator

We can easily add a method to Simulator that reports the appropriate interpolant order for the trajectory being generated, which is the order of the integrator that was used to advance the continuous intervals. Better would be to have the Simulator provide the interpolators since some integrators have built-in optimal interpolants. But for now I think just the order would be enough if you already have interpolators. (Currently we have orders 1,2, and 3.)

/cc @edrumwri

[edrumwri]

What kind of interpolators will you be using, Sam?

---

Evan Drumwright Research Scientist http://positronicslab.github.io Toyota Research Institute Palo Alto, CA

On Thu, Dec 29, 2016 at 3:16 PM, Michael Sherman notifications@github.com wrote:

I'm not precisely sure how one would get the interpolation scheme from the integrator

We can easily add a method to Simulator that reports the appropriate interpolant order for the trajectory being generated, which is the order of the integrator that was used to advance the continuous intervals. Better would be to have the Simulator provide the interpolators since some integrators have built-in optimal interpolants. But for now I think just the order would be enough if you already have interpolators. (Currently we have orders 1,2, and 3.)

/cc @edrumwri https://github.com/edrumwri

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/RobotLocomotion/drake/issues/3734#issuecomment-269694806, or mute the thread https://github.com/notifications/unsubscribe-auth/ACHwzzan0l9KoOz00aRz3bB1Q7Z9fAeCks5rNCNEgaJpZM4KSC1v .

[sammy-tri]

The simplest thing currently available is to create a PiecewisePolynomial using one of its existing interpolation methods (ZOH, FOH, Pchip, or Cubic). I'll defer to the Simulator2 crew for what the correct answer is (which may be a different means of constructing the PiecewisePolynomial or not using PiecewisePolynomial to do the interpolation at all).

[edrumwri]

I believe @sherm1 is the expert in obtaining "dense" output from integrators with order greater than one- I'll claim little expertise in this area, and will need to do a little research- but I believe Hermite interpolation is the go to approach. Care to comment @sherm1?

---

Evan Drumwright Research Scientist http://positronicslab.github.io Toyota Research Institute Palo Alto, CA

On Thu, Dec 29, 2016 at 3:59 PM, Sam Creasey notifications@github.com wrote:

The simplest thing currently available is to create a PiecewisePolynomial using one of its existing interpolation methods (ZOH, FOH, Pchip, or Cubic). I'll defer to the Simulator2 crew for what the correct answer is (which may be a different means of constructing the PiecewisePolynomial or not using PiecewisePolynomial to do the interpolation at all).

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/RobotLocomotion/drake/issues/3734#issuecomment-269700125, or mute the thread https://github.com/notifications/unsubscribe-auth/ACHwz_wxccT9n-BhvhiuJMY7sOWBnv8hks5rNC0vgaJpZM4KSC1v .

[RussTedrake]

The signal logger was a stop-gap measure; I'm not sure if it's the long-term solution. I think that (fundamentally), the leaf systems don't have access to as much information as the actual simulator --- for instance about event detection in addition to the type of interpolation. But this discussion is going in the right direction. I think that there should be a "correct" interpolation scheme for most every choice of integrators that faithfully represents the accuracy of the discrete approximation used during integration. I'm hoping to appeal to @sherm1's love for accuracy here. If we are going to analyze the results of simulation, we want to do it with the proper accuracy (and no more).

For discrete time systems, I think it should output a discrete trajectory (only defined at the sampling period). For continuous systems, it should output e.g. the hermite or pp trajectory of the appropriate order -- there should be a correct answer here.

[sherm1]

I added issue #4646 for adding interpolation capability to the Simulator.

[sammy-tri]

Do we think it's worth creating a temporary solution involving something like SignalLogger and PiecewisePolynomial where we get reduced accuracy playback until #4646 is done? I'm not sure I know the answer without knowing what the application for such a stop-gap would be.

[RussTedrake]

The signal logger (by itself) was my sufficient as a stop-gap solution for me.

[sammy-tri]

Ok, then I'm going to vote that we don't do anything else here #4646 is producing dense outputs and enhanced interpolation capabilities. Reducing priority/assignment scope.

[edrumwri]

This was provided by @hidmic in #8951.