diff --git a/src/groups/special_euclidean.jl b/src/groups/special_euclidean.jl index c23ceff3c1..f582da8a9f 100644 --- a/src/groups/special_euclidean.jl +++ b/src/groups/special_euclidean.jl @@ -739,26 +739,27 @@ function get_vector( basis::DefaultOrthogonalBasis, ) return ArrayPartition( - get_vector(M.manifold.manifolds[1].manifold, p.x[1], view(c, 1:2), basis), - get_vector(M.manifold.manifolds[2].manifold, p.x[2], c[3], basis), + get_vector(M.manifold.manifolds[1].manifold, p.x[1], c[SOneTo(2)], basis), + get_vector(M.manifold.manifolds[2].manifold, p.x[2], c[SA[3]], basis), ) end + function hat(M::SpecialEuclidean{3}, p::ArrayPartition, c) M1, M2 = M.manifold.manifolds return ArrayPartition( - get_vector_orthogonal(M1.manifold, p.x[1], view(c, 1:3), ℝ), - get_vector_orthogonal(M2.manifold, p.x[2], view(c, 4:6), ℝ), + get_vector_orthogonal(M1.manifold, p.x[1], c[SOneTo(3)], ℝ), + get_vector_orthogonal(M2.manifold, p.x[2], c[SA[4,5,6]], ℝ), ) end function get_vector( M::SpecialEuclidean{3}, p::ArrayPartition, - Xc::AbstractVector, + c::AbstractVector, basis::DefaultOrthogonalBasis, ) return ArrayPartition( - get_vector(M.manifold.manifolds[1].manifold, p.x[1], view(Xc, 1:3), basis), - get_vector(M.manifold.manifolds[2].manifold, p.x[2], view(Xc, 4:6), basis), + get_vector(M.manifold.manifolds[1].manifold, p.x[1], c[SOneTo(3)], basis), + get_vector(M.manifold.manifolds[2].manifold, p.x[2], c[SA[4,5,6]], basis), ) end function compose(::SpecialEuclidean, p::ArrayPartition, q::ArrayPartition)