Add ball joint to urdf parser (#53)

Author: jfkunz

src/ConstrainedDynamics.jl

@@ -45,6 +45,7 @@ export Origin,
     setPosition!,
     setVelocity!,
     setForce!,
+    addForce!,
     getid,
     getcomponent,
     getbody,
@@ -54,6 +55,7 @@ export Origin,
     initializeConstraints!,
     disassemble,
     minimalCoordinates,
+    minimalVelocities,
     activate!,
     deactivate!,
     linearsystem,

src/joints/joint.jl

@@ -37,6 +37,13 @@ g(joint::Joint, stateb::State) = g(joint, posargsc(stateb)...)
 end
 @inline setForce!(joint::Joint) = return
 
+
+@inline function addForce!(joint::Joint, Fτ::SVector)
+    joint.Fτ += zerodimstaticadjoint(nullspacemat(joint)) * Fτ
+    return
+end
+@inline addForce!(joint::Joint) = return
+
 ## Derivative wrappers
 @inline function ∂Fτ∂ua(joint::Joint, body1::Body, body2::Body)
     return ∂Fτ∂ua(joint, body1.state, body2.state) * zerodimstaticadjoint(nullspacemat(joint))

src/main_components/equalityconstraint.jl

@@ -108,7 +108,15 @@ end
 function setForce!(mechanism, eqc::EqualityConstraint{T,N,Nc}, Fτ::AbstractVector) where {T,N,Nc}
     @assert length(Fτ)==3*Nc-N
     for i = 1:Nc
-        setForce!(eqc.constraints[i], Fτ[SUnitRange(eqc.inds[i][1],eqc.inds[i][2])])
+        setForce!(eqc.constraints[i], Fτ[SUnitRange(eqc.inds[i][1], eqc.inds[i][2])])
+    end
+    return
+end
+
+function addForce!(mechanism, eqc::EqualityConstraint{T,N,Nc}, Fτ::AbstractVector) where {T,N,Nc}
+    @assert length(Fτ)==3*Nc-N
+    for i = 1:Nc
+        addForce!(eqc.constraints[i], Fτ[SUnitRange(eqc.inds[i][1], eqc.inds[i][2])])
     end
     return
 end

src/ui/urdf.jl

@@ -209,6 +209,8 @@ function parse_joint(xjoint, plink, clink, T)
         joint = EqualityConstraint(Fixed(plink, clink; p1=p1, qoffset = q), name=name)
     elseif jointtype == "floating"
         joint = EqualityConstraint(Floating(plink, clink), name=name)
+    elseif jointtype == "ball"
+        joint = EqualityConstraint(Spherical(plink, clink; p1=p1), name=name)
     else
         @error "Unknown joint type"
     end
@@ -239,6 +241,8 @@ function parse_loop_joint(xjoint, link1, link2, T)
         joint = EqualityConstraint(Fixed(link1, link2; p1=p1, p2=p2, qoffset = q1), name=name)
     elseif jointtype == "floating"
         joint = EqualityConstraint(Floating(link1, link2), name=name)
+    elseif jointtype == "ball"
+        joint = EqualityConstraint(Spherical(plink, clink; p1=p1, p2=p2), name=name)
     else
         @error "Unknown joint type"
     end