Add get minimal velocity function

Author: janbruedigam

src/joints/rotational.jl

@@ -256,3 +256,12 @@ end
     q = stateb.qc / joint.qoffset
     return nullspacemat(joint) * rotation_vector(q)
 end
+@inline function minimalVelocities(joint::Rotational, body1::Body, body2::Body)
+    statea = body1.state
+    stateb = body2.state
+    return nullspacemat(joint) * (vrotate(stateb.ωc,statea.qc\stateb.qc) - statea.ωc) # in body1's frame
+end
+@inline function minimalVelocities(joint::Rotational, body1::Origin, body2::Body)
+    stateb = body2.state
+    return nullspacemat(joint) * vrotate(stateb.ωc,stateb.qc) # in body1's frame
+end

src/joints/translational.jl

@@ -255,3 +255,12 @@ end
     stateb = body2.state
     return nullspacemat(joint) * g(joint, stateb.xc, stateb.qc)
 end
+@inline function minimalVelocities(joint::Translational, body1::Body, body2::Body)
+    statea = body1.state
+    stateb = body2.state
+    return nullspacemat(joint) * (stateb.vc - statea.vc)
+end
+@inline function minimalVelocities(joint::Translational, body1::Origin, body2::Body)
+    stateb = body2.state
+    return nullspacemat(joint) * stateb.vc
+end

src/main_components/equalityconstraint.jl

@@ -118,6 +118,11 @@ end
     return :(svcat($(vec...)))
 end
 
+@generated function minimalVelocities(mechanism, eqc::EqualityConstraint{T,N,Nc}) where {T,N,Nc}
+    vec = [:(minimalVelocities(eqc.constraints[$i], getbody(mechanism, eqc.parentid), getbody(mechanism, eqc.childids[$i]))) for i = 1:Nc]
+    return :(svcat($(vec...)))
+end
+
 @inline function GtλTof!(mechanism, body::Body, eqc::EqualityConstraint)
     isactive(eqc) && (body.state.d -= zerodimstaticadjoint(∂g∂ʳpos(mechanism, eqc, body.id)) * eqc.λsol[2])
     return