Major cleanup.

FullCordDynamics.jl

@@ -3,18 +3,18 @@ module FullCordDynamics
 # using TimerOutputs
 # const to = TimerOutput()
 
-using LinearAlgebra, StaticArrays#, Quaternions
+using LinearAlgebra, StaticArrays
 using StaticArrays: SUnitRange
 using Rotations
 
 using CoordinateTransformations
-using GeometryTypes: # Define geometric shapes
+using GeometryTypes:
     GeometryPrimitive, GeometryTypes, Vec, Point, Rectangle,
     HomogenousMesh, SignedDistanceField, HyperSphere, GLUVMesh, Pyramid
 using Blink
-using Colors: RGBA, RGB # Handle RGB colors
-using FileIO, MeshIO # Load meshes in MeshCat
-using MeshCat # Visualize 3D animations
+using Colors: RGBA, RGB
+using FileIO, MeshIO
+using MeshCat
 
 using Plots
 
@@ -28,6 +28,7 @@ export
     Constraint,
     Robot,
 
+    OriginConnection,
     Axis,
     Socket,
     SocketYZ,
@@ -45,23 +46,24 @@ include(joinpath("util", "util.jl"))
 include(joinpath("util", "customdict.jl"))
 include(joinpath("util", "quaternion.jl"))
 include(joinpath("util", "shapes.jl"))
-include(joinpath("components", "node.jl"))
+include(joinpath("components", "component.jl"))
 include(joinpath("joints", "joint.jl"))
 include(joinpath("components", "link.jl"))
 include(joinpath("components", "constraint.jl"))
 
 # include(joinpath("joints", "fixedposition.jl"))
 # include(joinpath("joints", "fixedorientation.jl"))
+include(joinpath("joints", "originconnection.jl"))
 include(joinpath("joints", "socket.jl"))
 include(joinpath("joints", "socketyz.jl"))
 include(joinpath("joints", "axis.jl"))
 
 include(joinpath("util", "graph.jl"))
 include(joinpath("util", "storage.jl"))
 
-include(joinpath("solver", "sparseldu2.jl"))
-include(joinpath("components", "robot.jl"))
 include(joinpath("solver", "sparseldu.jl"))
+include(joinpath("components", "robot.jl"))
+include(joinpath("solver", "solverfunctions.jl"))
 
 include(joinpath("solver", "newton.jl"))
 

components/component.jl

@@ -0,0 +1,10 @@
+abstract type Component{T} end
+
+#TODO do id differently?
+CURRENTID = -1
+getGlobalID() = (global CURRENTID-=1; return CURRENTID+1)
+resetGlobalID() = (global CURRENTID=-1; return)
+
+Base.show(io::IO, component::Component) = summary(io, component)
+
+@inline Base.foreach(f,itr::Vector{<:Component},arg...) = (for x in itr; f(x,arg...); end; return)

components/constraint.jl

@@ -1,69 +1,67 @@
-mutable struct Constraint{T,N,Nc,Cs} <: Node{T}
+mutable struct Constraint{T,N,Nc,Cs} <: Component{T}
     id::Int64
 
-    constr::Cs
+    constraints::Cs
     pid::Union{Int64,Nothing}
     linkids::SVector{Nc,Int64}
 
     s0::SVector{N,T}
     s1::SVector{N,T}
 
     function Constraint(jointdata...)
-        T = jointdata[1][1].T
-        linkids = Vector{Int64}(undef,0)
-        constr = Vector{Joint{T}}(undef,0)
+        T = getT(jointdata[1][1])#.T
+
         pid = jointdata[1][2]
+        linkids = Vector{Int64}(undef,0)
+        constraints = Vector{Joint{T}}(undef,0)
         N = 0
         for set in jointdata
-            push!(constr,set[1])
+            push!(constraints,set[1])
             @assert set[2] == pid
             push!(linkids,set[3])
-            N+=set[1].Nc
+            N += getNc(set[1])#.Nc
         end
-        constr = Tuple(constr)
-
-        Nc = length(constr)
-
-        id = getGlobalID()
+        constraints = Tuple(constraints)
+        Nc = length(constraints)
 
         s0 = @SVector zeros(T,N)
         s1 = @SVector zeros(T,N)
 
-        new{T,N,Nc,typeof(constr)}(id,constr,pid,linkids,s0,s1)
+        new{T,N,Nc,typeof(constraints)}(getGlobalID(),constraints,pid,linkids,s0,s1)
     end
 end
 
-Base.length(C::Constraint{T,N}) where {T,N} = N
+Base.length(c::Constraint{T,N}) where {T,N} = N
 
-@generated function g(robot,C::Constraint{T,N,Nc}) where {T,N,Nc}
-    vec = [:(g(C.constr[$i],getlink(robot,C.pid),getlink(robot,C.linkids[$i]),robot.dt,robot.No)) for i=1:Nc]
+@generated function g(c::Constraint{T,N,Nc},robot) where {T,N,Nc}
+    vec = [:(g(c.constraints[$i],getlink(robot,c.pid),getlink(robot,c.linkids[$i]),robot.dt,robot.No)) for i=1:Nc]
     :(vcat($(vec...)))
 end
 
-@inline function ∂g∂pos(robot,C::Constraint,id::Int64)
-    id == C.pid ? ∂g∂posa(robot,C,id) : ∂g∂posb(robot,C,id)
+@inline function ∂g∂pos(c::Constraint,id::Int64,robot)
+    id == c.pid ? ∂g∂posa(c,id,robot) : ∂g∂posb(c,id,robot)
 end
 
-@inline function ∂g∂vel(robot,C::Constraint,id::Int64)
-    id == C.pid ? ∂g∂vela(robot,C,id) : ∂g∂velb(robot,C,id)
+@inline function ∂g∂vel(c::Constraint,id::Int64,robot)
+    id == c.pid ? ∂g∂vela(c,id,robot) : ∂g∂velb(c,id,robot)
 end
 
-@generated function ∂g∂posa(robot,C::Constraint{T,N,Nc},id::Int64) where {T,N,Nc}
-    vec = [:(∂g∂posa(C.constr[$i],getlink(robot,id),getlink(robot,C.linkids[$i]),robot.No)) for i=1:Nc]
+@generated function ∂g∂posa(c::Constraint{T,N,Nc},id::Int64,robot) where {T,N,Nc}
+    vec = [:(∂g∂posa(c.constraints[$i],getlink(robot,id),getlink(robot,c.linkids[$i]),robot.No)) for i=1:Nc]
     return :(vcat($(vec...)))
 end
 
-@generated function ∂g∂posb(robot,C::Constraint{T,N,Nc},id::Int64) where {T,N,Nc}
-    vec = [:(∂g∂posb(C.constr[$i],getlink(robot,C.pid),getlink(robot,id),robot.No)) for i=1:Nc]
+@generated function ∂g∂posb(c::Constraint{T,N,Nc},id::Int64,robot) where {T,N,Nc}
+    vec = [:(∂g∂posb(c.constraints[$i],getlink(robot,c.pid),getlink(robot,id),robot.No)) for i=1:Nc]
     return :(vcat($(vec...)))
 end
 
-@generated function ∂g∂vela(robot,C::Constraint{T,N,Nc},id::Int64) where {T,N,Nc}
-    vec = [:(∂g∂vela(C.constr[$i],getlink(robot,id),getlink(robot,C.linkids[$i]),robot.dt,robot.No)) for i=1:Nc]
+@generated function ∂g∂vela(c::Constraint{T,N,Nc},id::Int64,robot) where {T,N,Nc}
+    vec = [:(∂g∂vela(c.constraints[$i],getlink(robot,id),getlink(robot,c.linkids[$i]),robot.dt,robot.No)) for i=1:Nc]
     return :(vcat($(vec...)))
 end
 
-@generated function ∂g∂velb(robot,C::Constraint{T,N,Nc},id::Int64) where {T,N,Nc}
-    vec = [:(∂g∂velb(C.constr[$i],getlink(robot,C.pid),getlink(robot,id),robot.dt,robot.No)) for i=1:Nc]
+@generated function ∂g∂velb(c::Constraint{T,N,Nc},id::Int64,robot) where {T,N,Nc}
+    vec = [:(∂g∂velb(c.constraints[$i],getlink(robot,c.pid),getlink(robot,id),robot.dt,robot.No)) for i=1:Nc]
     return :(vcat($(vec...)))
 end

components/link.jl

@@ -1,4 +1,4 @@
-abstract type AbstractLink{T} <: Node{T} end
+abstract type AbstractLink{T} <: Component{T} end
 
 mutable struct Link{T} <: AbstractLink{T}
     id::Int64
@@ -17,8 +17,6 @@ mutable struct Link{T} <: AbstractLink{T}
     f::SVector{6,T}
 
     function Link(m::T,J::AbstractArray{T,2}) where T
-        J = convert(SMatrix{3,3,T,9},J)
-
         x = [@SVector zeros(T,3)]
         q = [Quaternion{T}()]
 
@@ -39,71 +37,76 @@ mutable struct Link{T} <: AbstractLink{T}
     end
 end
 
-Base.length(C::Link) = 6
-
 mutable struct Origin{T} <: AbstractLink{T}
     id::Int64
 
     Origin{T}() where T = new{T}(getGlobalID())
 end
 
-function setInit!(link::Link{T}; x::AbstractVector{T}=zeros(T,3), q::Quaternion{T}=Quaternion{T}(),
-    F::AbstractVector{T}=zeros(T,3), τ::AbstractVector{T}=zeros(T,3)) where T
+
+Base.length(C::Link) = 6
+
+function setInit!(link::Link{T};
+    x::AbstractVector=zeros(T,3),
+    q::Quaternion=Quaternion{T}(),
+    F::AbstractVector=zeros(T,3),
+    τ::AbstractVector=zeros(T,3)) where T
 
     link.x[1] = convert(SVector{3,T},x)
     link.q[1] = q
     link.F[1] = convert(SVector{3,T},F)
     link.τ[1] = convert(SVector{3,T},τ)
-
+    return
 end
 
-function setInit!(link1::Link{T}, link2::Link{T}, p1,p2,; q::Quaternion{T}=Quaternion{T}(),
-    F::AbstractVector{T}=zeros(T,3), τ::AbstractVector{T}=zeros(T,3)) where T
+function setInit!(link1::Link{T}, link2::Link{T}, p1::AbstractVector, p2::AbstractVector;
+    q::Quaternion=Quaternion{T}(),
+    F::AbstractVector=zeros(T,3),
+    τ::AbstractVector=zeros(T,3)) where T
 
-    p1 = convert(SVector{3,T},p1)
-    p2 = convert(SVector{3,T},p2)
     x2 = link1.x[1] + vrotate(p1,link1.q[1]) - vrotate(p2,q)
-
     setInit!(link2; x=x2, q=q, F=F, τ=τ)
 end
 
-function setInit!(link1::Origin{T}, link2::Link{T}, p1,p2,; q::Quaternion{T}=Quaternion{T}(),
-    F::AbstractVector{T}=zeros(T,3), τ::AbstractVector{T}=zeros(T,3)) where T
+function setInit!(link1::Origin{T}, link2::Link{T}, p1::AbstractVector, p2::AbstractVector;
+    q::Quaternion=Quaternion{T}(),
+    F::AbstractVector=zeros(T,3),
+    τ::AbstractVector=zeros(T,3)) where T
 
-    p1 = convert(SVector{3,T},p1)
-    p2 = convert(SVector{3,T},p2)
     x2 = p1 - vrotate(p2,q)
-
     setInit!(link2; x=x2, q=q, F=F, τ=τ)
 end
 
-@inline getx3(link,dt) = getvnew(link)*dt + link.x[2]
-@inline getq3(link,dt) = Quaternion(dt/2*(Lmat(link.q[2])*ωbar(link,dt)))
-@inline getv1(link,dt) = (link.x[2]-link.x[1])/dt
-@inline function getω1(link,dt)
+@inline getx3(link::Link, dt) = getvnew(link)*dt + link.x[2]
+@inline getq3(link::Link, dt) = Quaternion(dt/2*(Lmat(link.q[2])*ωbar(link,dt)))
+@inline getv1(link::Link, dt) = (link.x[2]-link.x[1])/dt
+@inline function getω1(link::Link, dt)
     q1 = link.q[1]
     q2 = link.q[2]
     2/dt*(q1.s*imag(q2)-q2.s*imag(q1)-cross(imag(q1),imag(q2)))
-    # 2/dt*Vmat(LTmat(link.q[1]))*link.q[2]
+    # 2/dt*(VLᵀmat(link.q[1])*link.q[2])
 end
 
-@inline getvnew(link) = link.s1[SVector{3}(1:3)]
-@inline getωnew(link) = link.s1[SVector{3}(4:6)]
+#TODO use SOneTo and SUnitRange once they are faster
+# @inline getvnew(link::Link) = link.s1[SOneTo(3)]
+# @inline getωnew(link::Link) = link.s1[SUnitRange(4,6)]
+@inline getvnew(link::Link) = link.s1[SVector(1,2,3)]
+@inline getωnew(link::Link) = link.s1[SVector(4,5,6)]
 
-@inline function derivωbar(link::Link{T},dt) where T
+@inline function derivωbar(link::Link{T}, dt) where T
     ωnew = getωnew(link)
     msq = -sqrt(4/dt^2 - dot(ωnew,ωnew))
-    [ωnew'/msq;SMatrix{3,3,T,9}(I)]
+    [ωnew'/msq; SMatrix{3,3,T,9}(I)]
 end
-@inline function ωbar(link,dt)
+@inline function ωbar(link::Link, dt)
     ωnew = getωnew(link)
     Quaternion(sqrt(4/dt^2 - dot(ωnew,ωnew)),ωnew)
 end
 
-
-@inline function dynamics(robot, link::Link{T}) where T
+@inline function dynamics(link::Link{T}, robot) where T
     No = robot.No
     dt = robot.dt
+
     ezg = SVector{3,T}(0,0,-robot.g)
     dynT = link.m*((getvnew(link) - getv1(link,dt))/dt + ezg) - link.F[No]
 
@@ -116,16 +119,14 @@ end
 
     link.f = [dynT;dynR]
 
-    # for cid in connections(robot.graph,link.id)
-    #     # cid == -1 && break
-    #     GtλTof!(robot,getconstraint(robot,cid),link)
-    # end
+    for cid in connections(robot.graph,link.id)
+        GtλTof!(link,getconstraint(robot,cid),robot)
+    end
 
     return link.f
 end
 
-@inline function ∂dyn∂vel(robot, link::Link{T}) where T
-    dt = robot.dt
+@inline function ∂dyn∂vel(link::Link{T}, dt) where T
     J = link.J
     ωnew = getωnew(link)
     sq = sqrt(4/dt^2 - ωnew'*ωnew)
@@ -135,5 +136,5 @@ end
 
     Z = @SMatrix zeros(T,3,3)
 
-    return [[dynT Z];[Z dynR]]
+    return [[dynT; Z] [Z; dynR]]
 end