Fix sI

dojo-sim · Apr 13, 2023 · e763ce1 · e763ce1
1 parent 022eddd
commit e763ce1
Show file tree

Hide file tree

Showing 4 changed files with 5 additions and 5 deletions.
diff --git a/docs/src/contributing.md b/docs/src/contributing.md
@@ -9,7 +9,7 @@ Contributions are always welcome!
 ## Potentially Useful Contributions 
 Here are a list of current to-do's that would make awesome contributions:
 
-- reduce allocations by using StaticArrays in functions
+- reduce allocations by using StaticArrays and https://docs.julialang.org/en/v1/manual/profile/#Line-by-Line-Allocation-Tracking
 - improved parsing of URDF files
     - joint limits, friction coefficients
 - improved collision detection 

diff --git a/src/mechanism/state.jl b/src/mechanism/state.jl
@@ -69,7 +69,7 @@ function unpack_maximal_state(z::AbstractVector, i::Int)
 	zi = z[SUnitRange((i-1)*13+1,i*13)]
 	x2 = zi[SUnitRange(1,3)]
 	v15 = zi[SUnitRange(4,6)]
-	q2 = Quaternion(zi[7:10]...)
+	q2 = Quaternion(zi[SUnitRange(7,10)]...)
 	ω15 = zi[SUnitRange(11,13)]
 	return x2, v15, q2, ω15
 end

diff --git a/src/mechanism/urdf.jl b/src/mechanism/urdf.jl
@@ -177,7 +177,7 @@ function parse_link(xlink, materialdict, T; path_prefix)
         shape = nothing
     elseif length(shapes) > 1
         s = [s.shape for s in shapes]
-        shape = CombinedShapes(s, 0.0, szeros(3))
+        shape = CombinedShapes(s, 0.0, szeros(3,3))
     else
         shape = shapes[1]
     end

diff --git a/src/utilities/custom_static.jl b/src/utilities/custom_static.jl
@@ -18,8 +18,8 @@ sones(N)= @SVector ones(N)
 
 srand(N)= @SVector rand(N)
 
-sI(::Type{T}, N) where T = SMatrix{3,3,T}(I)
-sI(N) = SMatrix{3,3,Float64}(I)
+sI(::Type{T}, N) where T = SMatrix{N,N,T}(I)
+sI(N) = SMatrix{N,N,Float64}(I)
 
 # TODO: check StaticArray bug fix, then remove
 zerodimstaticadjoint(A) = A'