Merge pull request #80 from dojo-sim/jan/small_speedups

10-30% speed up on all mechanisms

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 

src/bodies/shapes.jl

@@ -326,7 +326,7 @@ mutable struct FrameShape{T} <: Shape{T}
             scale::AbstractVector=sones(3), 
             name::Symbol=Symbol("body_" * randstring(4)), color=RGBA(0.75, 0.75, 0.75))
         T = promote_type(quateltype.((m, position_offset, orientation_offset))...)
-        J = m * diagm([1;1;1])
+        J = m * sI(3)
         return Body(m, J; name=name, shape=new{T}(position_offset, orientation_offset, scale, color))
     end
 end

src/contacts/collisions/capsule_capsule.jl

@@ -79,9 +79,9 @@ function contact_point(relative::Symbol, collision::CapsuleCapsuleCollision, xp,
     # call mehrotra 
 
     if relative == :parent 
-        return collision.ip.z[0 .+ (1:3)]
+        return collision.ip.z[SA[1;2;3]]
     elseif relative == :child
-        return collision.ip.z[3 .+ (1:3)]
+        return collision.ip.z[SA[4;5;6]]
     end
 end
 

src/contacts/collisions/collision.jl

@@ -10,7 +10,7 @@ function contact_point_origin(x, q, k)
     x + vector_rotate(k, q)
 end
 
-∂contact_point_origin∂x(x, q, k) = 1.0 * I(3)
+∂contact_point_origin∂x(x, q, k) = 1.0 * sI(3)
 ∂contact_point_origin∂q(x, q, k) = ∂vector_rotate∂q(k, q)
 ∂contact_point_origin∂k(x, q, k) = rotation_matrix(q)
 

src/contacts/collisions/point_to_box.jl

@@ -109,7 +109,7 @@ function ∂contact_point_box∂x(ps, xc, qc, kx, ky, kz)
     ty = dot(ps - origin, v2) / dot(v2, v2) 
     tz = dot(ps - origin, v3) / dot(v3, v3)
 
-    X = 1.0 * I(3)
+    X = 1.0 * sI(3)
 
     if tx >= 1.0 
         # coc += v1 
@@ -205,33 +205,3 @@ function ∂contact_point_box∂q(ps, xc, qc, kx, ky, kz)
     return Q
     # FiniteDiff.finite_difference_jacobian(q -> contact_point_box(ps, xc, Quaternion(q...), kx, ky, kz), vector(qc))
 end
-
-# x_min = -1.0 
-# x_max = 1.0 
-# z_min = -1.0 
-# z_max = 1.0 
-
-# p = [-1.0; 1.0] #.- 1.0e-6
-
-# function klamp(t, a, b) 
-#     if t <= a 
-#         return a 
-#     elseif b <= t 
-#         return b 
-#     else
-#         da = abs(t - a) 
-#         db = abs(t - b)
-
-#         if da < db 
-#             return a 
-#         else
-#             return b
-#         end
-#     end
-# end
-
-# function nearest_point(p, x_min, x_max, z_min, z_max) 
-#     return [klamp(p[1], x_min, x_max), klamp(p[2], z_min, z_max)]
-# end
-
-# nearest_point(p, x_min, x_max, z_min, z_max)

src/contacts/collisions/point_to_box_v2.jl

@@ -43,33 +43,3 @@ function ∂contact_point_box∂q(ps, xc, qc, kx, ky, kz)
 
     FiniteDiff.finite_difference_jacobian(q -> contact_point_box(ps, xc, Quaternion(q...), kx, ky, kz), vector(qc))
 end
-
-# x_min = -1.0 
-# x_max = 1.0 
-# z_min = -1.0 
-# z_max = 1.0 
-
-# p = [-1.0; 1.0] #.- 1.0e-6
-
-# function klamp(t, a, b) 
-#     if t <= a 
-#         return a 
-#     elseif b <= t 
-#         return b 
-#     else
-#         da = abs(t - a) 
-#         db = abs(t - b)
-
-#         if da < db 
-#             return a 
-#         else
-#             return b
-#         end
-#     end
-# end
-
-# function nearest_point(p, x_min, x_max, z_min, z_max) 
-#     return [klamp(p[1], x_min, x_max), klamp(p[2], z_min, z_max)]
-# end
-
-# nearest_point(p, x_min, x_max, z_min, z_max)

src/contacts/collisions/point_to_segment.jl

@@ -125,85 +125,4 @@ function ∂contact_point_segment∂q(ps, xc, qc, ka, kb)
 
         return Q
     end
-end
-
-# # case 1
-# ps = [-2.0 - 0.0e-8; 0.0; 1.0] 
-# xc = [0.0; 0.0; 0.0]
-# qc = Quaternion([1.0; 0.0; 0.0; 0.0])
-# ka = [1.0; 0.0; 0.0]
-# kb = [-1.0; 0.0; 0.0]
-
-# contact_point_segment(ps, xc, qc, ka, kb)
-
-# ∂contact_point_segment∂p(ps, xc, qc, ka, kb)
-# FiniteDiff.finite_difference_jacobian(p -> contact_point_segment(p, xc, qc, ka, kb), ps)
-
-# ∂contact_point_segment∂x(ps, xc, qc, ka, kb)
-# FiniteDiff.finite_difference_jacobian(x -> contact_point_segment(ps, x, qc, ka, kb), xc)
-
-# ∂contact_point_segment∂q(ps, xc, qc, ka, kb)
-# FiniteDiff.finite_difference_jacobian(q -> contact_point_segment(ps, xc, Quaternion(q...), ka, kb), vector(qc))
-
-# # case 2
-# ps = [1.0 + 100.0e-8; 0.0; 1.0] 
-# xc = [0.0; 0.0; 0.0]
-# qc = Quaternion([1.0; 0.0; 0.0; 0.0])
-# ka = [1.0; 0.0; 0.0]
-# kb = [-1.0; 0.0; 0.0]
-
-# contact_point_segment(ps, xc, qc, ka, kb)
-
-# ∂contact_point_segment∂p(ps, xc, qc, ka, kb)
-# FiniteDiff.finite_difference_jacobian(p -> contact_point_segment(p, xc, qc, ka, kb), ps)
-
-# ∂contact_point_segment∂x(ps, xc, qc, ka, kb)
-# FiniteDiff.finite_difference_jacobian(x -> contact_point_segment(ps, x, qc, ka, kb), xc)
-
-# ∂contact_point_segment∂q(ps, xc, qc, ka, kb)
-# FiniteDiff.finite_difference_jacobian(q -> contact_point_segment(ps, xc, Quaternion(q...), ka, kb), vector(qc))
-
-# # case 3
-# ps = [0.0; 0.0; 1.0] 
-# xc = [0.0; 0.0; 0.0]
-# qc = Quaternion([1.0; 0.0; 0.0; 0.0])
-# ka = [1.0; 0.0; 0.0]
-# kb = [-1.0; 0.0; 0.0]
-
-# contact_point_segment(ps, xc, qc, ka, kb)
-
-# ∂contact_point_segment∂p(ps, xc, qc, ka, kb)
-# FiniteDiff.finite_difference_jacobian(p -> contact_point_segment(p, xc, qc, ka, kb), ps)
-
-# ∂contact_point_segment∂x(ps, xc, qc, ka, kb)
-# FiniteDiff.finite_difference_jacobian(x -> contact_point_segment(ps, x, qc, ka, kb), xc)
-
-# ∂contact_point_segment∂q(ps, xc, qc, ka, kb)
-# FiniteDiff.finite_difference_jacobian(q -> contact_point_segment(ps, xc, Quaternion(q...), ka, kb), vector(qc))
-
-
-
-# function get_t(a, b) 
-#     return a' * b ./ (b' * b)
-# end
-
-# function dget_tda(a, b) 
-#     return b' ./ (b' * b)
-# end
-
-# function dget_tdb(a, b) 
-#     D = a' ./ (b' * b)
-#     D += -1.0 * a' * b ./ (b' * b)^2 * 2.0 * b' 
-#     return D
-# end
-
-# a = rand(3) 
-# b = rand(3)
-
-# get_t(a, b)
-
-# dget_tda(a, b)
-# FiniteDiff.finite_difference_jacobian(x -> get_t(x, b), a)
-
-# dget_tdb(a, b) 
-# FiniteDiff.finite_difference_jacobian(x -> get_t(a, x), b)
+end

src/contacts/collisions/sphere_halfspace.jl

@@ -63,7 +63,7 @@ end
 function ∂contact_point∂x(relative::Symbol, jacobian::Symbol, collision::SphereHalfSpaceCollision, xp, qp, xc, qc)
     if relative == :parent 
         if jacobian == :parent 
-            return 1.0 * I(3)
+            return 1.0 * sI(3)
         elseif jacobian == :child 
             return szeros(eltype(xp), 3, 3)
         end

src/contacts/cone.jl

@@ -4,7 +4,7 @@ function cone_product(u::AbstractVector{T}, v::AbstractVector{T}) where {T}
 end
 
 function cone_product(u::SVector{N,T}, v::SVector{N,T}) where {N,T}
-    vcat(u' * v, u[1] * v[SVector{N-1}(2:end)] + v[1] * u[SVector{N-1}(2:end)])
+    vcat(u' * v, u[1] * v[SUnitRange(2,end)] + v[1] * u[SUnitRange(2,end)])
 end
 
 function cone_product_jacobian(u::SVector{3,T}) where T

src/contacts/contact.jl

@@ -125,7 +125,7 @@ function impulse_map_jacobian(relative::Symbol, jacobian::Symbol, model::Contact
     end
 
     Qx = rotation_matrix(inv(q)) * skew(r) * Xx 
-    Qx -= rotation_matrix(inv(q)) * skew(X * λ) * (∂contact_point∂x(relative, jacobian, model.collision, xp, qp, xc, qc) - (relative == jacobian ? 1.0 : 0.0) * I(3)) 
+    Qx -= rotation_matrix(inv(q)) * skew(X * λ) * (∂contact_point∂x(relative, jacobian, model.collision, xp, qp, xc, qc) - (relative == jacobian ? 1.0 : 0.0) * sI(3)) 
 
     Qq = rotation_matrix(inv(q)) * skew(r) * Xq 
     Qq -= rotation_matrix(inv(q)) * skew(X * λ) * ∂contact_point∂q(relative, jacobian, model.collision, xp, qp, xc, qc) 

src/contacts/linear.jl

@@ -90,8 +90,8 @@ function constraint(mechanism, contact::ContactConstraint{T,N,Nc,Cs,N½}) where
     sγ = contact.impulses_dual[2][1]
     ψ = contact.impulses[2][2]
     sψ = contact.impulses_dual[2][2]
-    β = contact.impulses[2][@SVector [3,4,5,6]]
-    sβ = contact.impulses_dual[2][@SVector [3,4,5,6]]
+    β = contact.impulses[2][SA[3;4;5;6]]
+    sβ = contact.impulses_dual[2][SA[3;4;5;6]]
 
     SVector{N½,T}(
         d - sγ,

src/contacts/nonlinear.jl

@@ -71,7 +71,7 @@ function constraint(mechanism, contact::ContactConstraint{T,N,Nc,Cs,N½}) where
     SVector{N½,T}(
         d - s[1],
         model.friction_coefficient * γ[1] - γ[2],
-        (model.friction_parameterization * vt - s[@SVector [3,4]])...)
+        (model.friction_parameterization * vt - s[SA[3;4]])...)
 end
 
 function constraint_jacobian(contact::ContactConstraint{T,N,Nc,Cs,N½}) where {T,N,Nc,Cs<:NonlinearContact{T,N},N½}

src/contacts/velocity.jl

@@ -12,7 +12,7 @@ function ∂contact_point_velocity∂q(model::Contact, x, q, v, ω, c)
 end
 
 function ∂contact_point_velocity∂v(model::Contact, x, q, v, ω, c)
-    return 1.0 * I(3)
+    return 1.0 * sI(3)
 end
 
 function ∂contact_point_velocity∂ω(model::Contact, x, q, v, ω, c)

src/gradients/utilities.jl

@@ -25,7 +25,7 @@ function attitude_jacobian(data::AbstractVector, Nb::Int)
     for i = 1:Nb
         x2, v15, q2, ω15 = unpack_data(data[13 * (i-1) .+ (1:13)])
         q2 = Quaternion(q2...)
-        G = cat(G, I(6), LVᵀmat(q2), I(3), dims = (1,2))
+        G = cat(G, sI(6), LVᵀmat(q2), sI(3), dims = (1,2))
     end
     ndata = length(data)
     nu = ndata - size(G)[1]
@@ -34,10 +34,10 @@ function attitude_jacobian(data::AbstractVector, Nb::Int)
 end
 
 function unpack_data(data::AbstractVector)
-    x2 = data[SVector{3,Int}(1:3)]
-    v15 = data[SVector{3,Int}(4:6)]
-    q2 = data[SVector{4,Int}(7:10)]
-    ω15 = data[SVector{3,Int}(11:13)]
+    x2 = data[SA[1;2;3]]
+    v15 = data[SA[4;5;6]]
+    q2 = data[SA[7;8;9;10]]
+    ω15 = data[SA[11;12;13]]
     return x2, v15, q2, ω15
 end
 

src/joints/constraints.jl

@@ -99,16 +99,24 @@ function Base.show(io::IO, mime::MIME{Symbol("text/plain")}, constraint::JointCo
 end
 
 # constraints
-@generated function constraint(mechanism, joint::JointConstraint)
-    pbody = :(get_body(mechanism, joint.parent_id))
-    cbody = :(get_body(mechanism, joint.child_id))
-    tra = :(constraint(joint.translational,
-        $pbody, $cbody,
-        joint.impulses[2][joint_impulse_index(joint,1)], mechanism.μ, mechanism.timestep))
-    rot = :(constraint(joint.rotational,
-        $pbody, $cbody,
-        joint.impulses[2][joint_impulse_index(joint,2)], mechanism.μ, mechanism.timestep))
-    return :(svcat($tra, $rot))
+# @generated function constraint(mechanism, joint::JointConstraint)
+#     pbody = :(get_body(mechanism, joint.parent_id))
+#     cbody = :(get_body(mechanism, joint.child_id))
+#     tra = :(constraint(joint.translational,
+#         $pbody, $cbody,
+#         joint.impulses[2][joint_impulse_index(joint,1)], mechanism.μ, mechanism.timestep))
+#     rot = :(constraint(joint.rotational,
+#         $pbody, $cbody,
+#         joint.impulses[2][joint_impulse_index(joint,2)], mechanism.μ, mechanism.timestep))
+#     return :(svcat($tra, $rot))
+# end
+
+function constraint(mechanism, joint::JointConstraint)
+    pbody = get_body(mechanism, joint.parent_id)
+    cbody = get_body(mechanism, joint.child_id)
+    tra = constraint(joint.translational, pbody, cbody, joint.impulses[2][joint_impulse_index(joint,1)], mechanism.μ, mechanism.timestep)
+    rot = constraint(joint.rotational, pbody, cbody, joint.impulses[2][joint_impulse_index(joint,2)], mechanism.μ, mechanism.timestep)
+    return svcat(tra, rot)
 end
 
 # # constraints Jacobians
@@ -403,7 +411,7 @@ function get_joint_impulses(joint::JointConstraint{T,N,Nc}, i::Int) where {T,N,N
     end
     n2 = n1 - 1 + impulses_length((joint.translational, joint.rotational)[i])
 
-    λi = SVector{n2-n1+1,T}(joint.impulses[2][n1:n2])
+    λi = SVector{n2-n1+1,T}(joint.impulses[2][SUnitRange(n1,n2)])
     return λi
 end
 

src/joints/joint.jl

@@ -134,8 +134,8 @@ limits_length(joint::Joint{T,Nλ,Nb}) where {T,Nλ,Nb} = Nb
 impulses_length(joint::Joint{T,Nλ,Nb,N}) where {T,Nλ,Nb,N} = N
 
 function split_impulses(joint::Joint{T,Nλ,Nb}, η) where {T,Nλ,Nb}
-    s = η[SVector{Nb,Int}(1:Nb)]
-    γ = η[SVector{Nb,Int}(Nb .+ (1:Nb))]
+    s = η[SUnitRange(1,Nb)]
+    γ = η[SUnitRange(Nb+1,2*Nb)]
     return s, γ
 end
 

src/joints/minimal.jl

@@ -168,10 +168,10 @@ function set_minimal_coordinates_velocities!(joint::JointConstraint,
     Atra = zerodimstaticadjoint(nullspace_mask(tra))
 
     # parent state
-    xa = SVector{3}(zp[1:3])
-    va = SVector{3}(zp[3 .+ (1:3)])
-    qa = Quaternion(zp[6 .+ (1:4)]...)
-    ωa = SVector{3}(zp[10 .+ (1:3)])
+    xa = SVector{3}(zp[SA[1;2;3]])
+    va = SVector{3}(zp[SA[4;5;6]])
+    qa = Quaternion(zp[SA[7;8;9;10]]...)
+    ωa = SVector{3}(zp[SA[11;12;13]])
 
     # positions
     Δq = axis_angle_to_quaternion(Arot * Δθ)
@@ -236,7 +236,7 @@ function minimal_coordinates_velocities_jacobian_parent(joint::JointConstraint{T
 
     # Jacobians
 
-    ∂xb∂xa = 1.0 * I(3)
+    ∂xb∂xa = 1.0 * sI(3)
 
     ∂xb∂va = szeros(T, 3, 3)
 
@@ -255,7 +255,7 @@ function minimal_coordinates_velocities_jacobian_parent(joint::JointConstraint{T
 
     ∂vb∂xa = szeros(T, 3, 3)
 
-    ∂vb∂va = 1.0 * I(3)
+    ∂vb∂va = 1.0 * sI(3)
 
     ∂vb∂qa = 1.0 / timestep * ∂vector_rotate∂q(pa + Atra * Δx, qa)
     ∂vb∂qa -= 1.0 / timestep * ∂vector_rotate∂q(pb, qb) * Rmat(orientation_offset * Δq)

src/joints/rotational/dampers.jl

@@ -13,11 +13,11 @@ function damper_force(relative::Symbol, joint::Rotational{T}, qa::Quaternion, ω
 
     velocity = minimal_velocities(joint, szeros(3), szeros(3), qa, ωa, szeros(3), szeros(3), qb, ωb, timestep)
     if relative == :parent
-        force = 1.0000 * damper * Aᵀ * velocity # currently assumes same damper constant in all directions
+        force = 1.0 * damper * Aᵀ * velocity # currently assumes same damper constant in all directions
         rotate && (force = vector_rotate(force, orientation_offset)) # rotate back to frame a
         return [szeros(T, 3); force]
     elseif relative == :child
-        force = - 1.0000 * damper * Aᵀ * velocity # currently assumes same damper constant in all directions
+        force = - 1.0 * damper * Aᵀ * velocity # currently assumes same damper constant in all directions
         rotate && (force = vector_rotate(force, inv(qb) * qa * orientation_offset)) # rotate back to frame b
         return [szeros(T, 3); force]
     end