toXf :: ( Vectorize x, Vectorize z, Vectorize u, Vectorize p
        , KnownNat n, KnownNat deg
        ) => J (CollTraj x z u p n deg) (Vector Double)-> x Double
toXf traj = splitJV xf
  where
    CollTraj _ _ _ xf = split traj
toXf :: ( Vectorize x, Vectorize z, Vectorize u, Vectorize p
        , KnownNat n, KnownNat deg
        ) => J (CollTraj x z u p n deg) (Vector Double)-> x Double
toXf traj = splitJV xf
  where
    CollTraj _ _ _ xf = split traj
toXf traj = splitJV xf
  where
    CollTraj _ _ _ xf = split traj