Add methods for converting trajectories between different (but equivalent) state spaces

[brianhou]

We need to be able to plan trajectories for one MetaSkeleton in MetaSkeletonStateSpace, while being able to execute them in different but equivalent state spaces. This is important for pipelining planning and execution: since trajectories are tied to a specific state space (a MetaSkeletonStateSpace for a MetaSkeleton in a specific World), trajectories from the planning environment cannot be executed in the execution environment.

This should probably have better error-checking to prevent converting between incompatible StateSpaces. Without knowing the exact type of the StateSpace, is the best thing I can do here just compare the dimensions? Is it fair to assume that the two spaces will always be a MetaSkeletonStateSpace? (When else would one need to convert the state space into an equivalent one?)

I'm open to better suggestions, but this is the best way that I've thought of so far.

---

Before creating a pull request

• Document new methods and classes
• Format code with make format

Before merging a pull request

• Set version target by selecting a milestone on the right side
• Summarize this change in CHANGELOG.md
• Add unit test(s) for this change

[codecov]

Codecov Report

Merging #249 into master will not change coverage.
The diff coverage is 100%.

@@           Coverage Diff           @@
##           master     #249   +/-   ##
=======================================
  Coverage   70.43%   70.43%           
=======================================
  Files         181      181           
  Lines        5391     5391           
  Branches      847      847           
=======================================
  Hits         3797     3797           
  Misses       1074     1074           
  Partials      520      520

Impacted Files	Coverage Δ
src/statespace/GeodesicInterpolator.cpp	85.71% <ø> (ø)	⬆️
include/aikido/statespace/Interpolator.hpp	100% <ø> (ø)	⬆️
include/aikido/trajectory/Trajectory.hpp	100% <ø> (ø)	⬆️
include/aikido/statespace/GeodesicInterpolator.hpp	100% <ø> (ø)	⬆️
src/trajectory/Spline.cpp	82.4% <ø> (ø)	⬆️
include/aikido/trajectory/Interpolated.hpp	100% <ø> (ø)	⬆️
include/aikido/trajectory/Spline.hpp	100% <100%> (ø)	⬆️
src/trajectory/Interpolated.cpp	75.3% <100%> (ø)	⬆️

[psigen]

Hmm, I need to think a bit about how we are ending up a situation where we have multiple Statespaces that are effectively identical. We would eventually need conversions for situations where there are two statespaces from different places that happen to be identical, but it seems weird that these multiple Statespaces are being created through cloning.

Is there something going on where would prefer these Statespaces to be immutable objects that are passed by reference instead of copied when cloning occurs? @mkoval?

[mkoval]

This is a fundamental issue beyond MetaSkeletonStateSpace. Aikido currently enforces that a State may only be used with the StateSpace instance that created it. In theory, this means that it is not valid to pass a state created by one R<3> instance to another R<3> instance - even though they are bit-for-bit identical.

I see two possible solutions:

1. allow a StateSpace to operate on States from other “equivalent” StateSpace instances or
2. add functions to convert States between “equivalent” state spaces (this PR).

The key difference between is whether we (1) relax this requirement or (2) work around it through conversion functions. I prefer (1) because it keeps all of the nasty conversion logic, instead of scattered throughout the entire public API.

The conversion and compatibility check could look something like this:

bool Rn::convert(StateSpace sourceSpace, const StateSpace::State* sourceState,
               StateSpace::State* destinationState)
{
  auto sourceSpaceRn = dynamic_cast<const Rn*>(&sourceSpace);
  if (!(sourceSpaceRn && mN == sourceSpaceRn->mN)) return false;
  auto sourceStateRn = static_cast<const Rn*>(sourceState);
  auto destinationStateRn = static_cast<Rn*>(destinationState);
  destinationStateRn->mData = sourceStateRn->mData;
  return true;
}

In principle, this solves the problem. All we would need to do is implement a conversion function on each JointStateSpace that checks that the two Joints are “equivalent” and copies their position vector. Since a StateSpace only represent the topology of the space, we could deem two Joints equivalent if they: (a) are the same type and (b) have the same number of DegreeOfFreedoms.

---

However, @psigen raises a great point. It’s odd that we a MetaSkeletonStateSpace is tied to a specific MetaSkeleton, even though it only represents the topology of the state space. Putting a MetaSkeleton and Joint objects inside the space muddles this with a huge number of attributes (names, owners, etc) that are semantically important but do not affect the topology of the space at all. This seriously confuses what it means to be equivalent - which of these properties matter for a given application?

It may be worth refactoring MetaSkeletonStateSpace to be a factory that: (1) provides a factory function to create a StateSpace and (2) provides helper functions for converting between MetaSkeleton positions and the States. That way we could use the same StateSpace instance across multiple MetaSkeletons that have the same topology and dodge this issue entirely.

For example, the API could look like this:

class MetaSkeletonStateSpace {
  StateSpacePtr getStateSpace() const;
  void convertStateToPositions(const StateSpace::State* state, Eigen::VectorXd* positions) const;
  void convertPositionsToState(const Eigen::VectorXd& positions, StateSpace::State* state) const;

  static MetaSkeletonStateSpace fromMetaSkeleton(const dart::dynamics::MetaSkeleton& metaSkeleton);
};

In hindsight, I wish I implemented it this way originally. 😓

---

@brianhou @psigen @jslee02: Thoughts?

[psigen]

Aikido currently enforces that a State may only be used with the StateSpace instance that created it.

@mkoval: Do you remember why we originally went with this? I'm trying to recall if it was an implementation constraint, or if there was a particular set of semantics we were trying to create.

It certainly seems intuitive at the moment that two R<3> instances should be able to operate on the same State, i.e. that this could be enforced at the class level instead of the instance level.

---

Separately, I'm still not entirely clear why we need to create new instances of StateSpace so often. It seems like we could re-use and pass around the same StateSpace instance for most cloning and planning operations (assuming we decouple the MetaSkeletonStateSpace, which seems like it really shouldn't be inheriting anyway, since, as @mkoval pointed out, it's more of an adapter/wrapper from StateSpace to a MetaSkeleton's Joint positions and ties in a lot of non-topology-related properties).

[mkoval]

@mkoval: Do you remember why we originally went with this? I'm trying to recall if it was an implementation constraint, or if there was a particular set of semantics we were trying to create.

It certainly seems intuitive at the moment that two R<3> instances should be able to operate on the same State, i.e. that this could be enforced at the class level instead of the instance level.

It's a semantic I wanted to enforce.

It's true that it's harmless to pass states between two R<3> instances because all R<3> instances are identical. The same is not true for state spaces that take runtime arguments, e.g. to support conversions between Rn instances or R<x> and Rn(x)` instances.

It gets more complicated if you think about state spaces that represent the same topology, but have different internal representations. For example, SO(3) may be parameterized by unit quaternions, spatial twists, rotation matrices, or Euler angles. It's clear what the intended result is - that all of these parameterzations are interchangeably - but it's tricky to implement that cleanly. @jslee02 tacked some of these issues in the GenericJoint refactor in DART.

The simplest solution is to only allow a StateSpace to operate on States that it creates, so that is what we did to avoid stalling development. Now we have to figure out a better solution. 😁

---

I am generally in favor of keeping the "only allow a StateSpace to operate on States that it creates" semantic by adding an extra conversion function. I see two key advantages:

• Knowing if a State is compatible with a StateSpace requires knowledge of what StateSpace created it (e.g. Rn::State does not know its own length)
• Converting between compatible states may be inefficient, e.g. converting between a rotation matrix and a unit quaternion requires, so the conversion should be explicit.
• The compatibility rules are complicated and should live in one place, rather than in every function that is part of the StateSpace's public API.

---

Separately, I'm still not entirely clear why we need to create new instances of StateSpace so often. It seems like we could re-use and pass around the same StateSpace instance for most cloning and planning operations (assuming we decouple the MetaSkeletonStateSpace, which seems like it really shouldn't be inheriting anyway [...]

I agree! Even without a conversion function, a lot of issues go away if we do this. We could also consider using a singleton for state spaces that take no runtime parameters (e.g. R<n>).

[psigen]

Ok, FWIW, I think we're on the same page here.

I think it would make sense to do the following:

1. Refactor non-dynamic state space (e.g. R<3>) to use singleton classes (perhaps make the constructor private and add a static .create() on the template class?
2. Change most planning and cloning operations to pass pointers to the same instance of the StateSpace instead of creating a new instance.

If we do these two things, then I think we largely obviate the need for this PR. We might need to convert state spaces in cases where two dynamic StateSpaces are loaded and we need to do a more rigorous equivalence check, but we can do this via a "conversion" function ahead of time and use a normal comparator to check instance equality in the internal functions.

@brianhou @mkoval does this follow what you were thinking?

[brianhou]

@mkoval -- @gilwoolee and I have been trying to understand how this works, but we have a few questions.

1. What kind of StateSpace would getStateSpace return? From our understanding, we would then need to then create an additional StateSpace class. Is it necessary to enforce this separation?

2. What's the benefit of having a static method to create a MetaSkeletonStateSpace, rather than an ordinary constructor? We thought that maybe passing it into the constructor implies that the MetaSkeleton is being saved in one of the fields, and fromMetaSkeleton doesn't have that implication? Regardless of where we put the logic, it seems that we would still be constructing a CartesianProduct of JointStateSpaces -- is that correct?

---

Rather than having a separate StateSpace class, what if we continued to have MetaSkeletonStateSpace inherit from StateSpace? I think our proposal basically looks the same as the current implementation, except that we no longer have a mMetaSkeleton field maintained in the MetaSkeletonStateSpace class. (Similarly, we would remove mJoint from JointStateSpaces.)

We'd still use the input MetaSkeleton to ensure that MetaSkeletonStateSpace is a CartesianProduct of the appropriate JointStateSpaces, but we wouldn't keep a reference to it. Every time we would need to operate on a concrete MetaSkeleton or Joint, we'd explicitly pass in the object we want to operate on to our getState and setState convenience methods.

Since MetaSkeletonStateSpaces won't have any concrete MetaSkeleton, some classes that currently use MetaSkeletonStateSpace (e.g. the CollisionFree constraint) would then have to take an additional MetaSkeleton argument in their constructor.

class JointStateSpace : StateSpace {
  void convertStateToPositions(const State* state, Eigen::VectorXd* positions) const;
  void convertPositionsToState(const Eigen::VectorXd& positions, State* state) const;

  void getState(Joint skel, State* state) const;
  void setState(Joint skel, const State* state);
};

class MetaSkeletonStateSpace : CartesianProduct {
  void convertStateToPositions(const State* state, Eigen::VectorXd* positions) const;
  void convertPositionsToState(const Eigen::VectorXd& positions, State* state) const;

  void getState(MetaSkeleton skel, State* state) const;
  void setState(MetaSkeleton skel, const State* state);

  static MetaSkeletonStateSpace fromMetaSkeleton(const dart::dynamics::MetaSkeleton& metaSkeleton);
};

---

1. Refactor non-dynamic state space (e.g. R<3>) to use singleton classes (perhaps make the constructor private and add a static .create() on the template class?
2. Change most planning and cloning operations to pass pointers to the same instance of the StateSpace instead of creating a new instance.

If we do these two things, then I think we largely obviate the need for this PR.

My understanding is that this solution means we won't implement the convert logic (and won't need to), since we're sticking to the stricter requirement that States have to be from the exact StateSpace instance. Making the non-DART state spaces singletons sounds reasonable, and we haven't really started using the cloning so we can definitely just start using the same state space there.

[psigen]

@brianhou @gilwoolee: maybe it would help to think of it this way:

MetaSkeletons are used to generate MetaSkeletonStateSpaces, but ideally, the two have nothing to do with each other besides corresponding at the time of construction.

So, you can think of MetaSkeletonStateSpace as just a StateSpace, that happens to have a couple of conversion functions on it that go from positions to States. But it doesn't track what MetaSkeleton created it, and it doesn't keep up to date with the generating MetaSkeleton (it's immutable). Instead, it specifically only keeps mapping information from the JointStateSpaces that were available in the MetaSkeleton at construction time.

We can take a given MetaSkeleton and generate up this immutable MetaSkeletonStateSpace and keep it around to use for functions that need a state space (e.g. planning). If something changes in the MetaSkeleton, we need to generate a new MetaSkeletonStateSpace (after all, we can't convert our old States to the new Joints anyway). If we make a copy of the MetaSkeleton, we don't need to generate a new MetaSkeletonStateSpace: nothing changed, so we can just pass a pointer to the instance we already have.

So the intended API for a MetaSkeletonStateSpace is for it to inherit from StateSpace, but not retain information about the originating MetaSkeleton. That information is only used during construction (e.g. in the create() method), to build an internal mapping of JointStateSpace which is used after that. You can think of it as a StateSpace that is formed from a snapshot of the MetaSkeleton at that time.

Does that make sense? With that architecture the presented API was intended to be relatively straightforward to implement without requiring extra params or conversion functions.

[mkoval]

I entirely agree with @psigen here. I regret not implementing MetaSkeletonStateSpace that way in the first place. 😞 I strongly suggest tabling the discussion of converting States between StateSpace instances for now and instead focus on making that refactor, which solves the same problem in a much cleaner way.

[brianhou]

Sounds good! I started to work on the refactor today, and I'm hoping to finish it in the next couple days.

I initially removed the Joint completely, but it seems that it would still be convenient to maintain some information about the joint in the JointStateSpaces. I'm currently planning to keep a copy of Joint::Properties in the JointStateSpaces so that we can still extract information like position limits for creating constraints.

[brianhou]

Closing in favor of #278.