Merge branch 'prepare_ljf_merge' into dev

# Conflicts: # examples/robots/jixiebi.jl # src/mechanics/contact.jl # src/mechanics/dynamics.jl # src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_CCP_constant_mass_hole.jl # src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_constant_mass_hole.jl # src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_sliding_cable_FB.jl # src/members/apparatus.jl # src/postprocess/vis.jl # src/structures/constraints.jl # src/structures/linearization.jl # src/structures/structure.jl # test/vis.jl # yard/jixiebi/Group2_1.jl
Rible-Sim · May 21, 2024 · 4eb0fa7 · 4eb0fa7
2 parents 850661f + 7432a13
commit 4eb0fa7
Show file tree

Hide file tree

Showing 21 changed files with 875 additions and 1,013 deletions.
diff --git a/examples/robots/jixiebi.jl b/examples/robots/jixiebi.jl
@@ -833,7 +833,7 @@ function build_4_jixiebis(n; ks1=8.0, restlens1=0.018007,
         [len42, r2 * sqrt(3) / 2, -r2 / 2],
         [len42, -r2 * sqrt(3) / 2, -r2 / 2]]
 
-    rigid_color = :teal
+    rigid_color = :darkgreen
     mesh1 = load(RB.assetpath("jixiebi/m1.STL")) |> RB.make_patch(; rot=RotZ(-π / 2)) |> RB.make_patch(; rot=RotX(pi / 6), scale=1 / scale, color=rigid_color)
     mesh2 = load(RB.assetpath("jixiebi/m2.STL")) |> RB.make_patch(; rot=RotZ(-π / 2)) |> RB.make_patch(; rot=RotX(pi / 6), scale=1 / scale, color=rigid_color)
     mesh3 = load(RB.assetpath("jixiebi/m3.STL")) |> RB.make_patch(; rot=RotZ(-π / 2)) |> RB.make_patch(; rot=RotX(pi / 6), scale=1 / scale, color=rigid_color)

diff --git a/src/mechanics/contact.jl b/src/mechanics/contact.jl
@@ -313,7 +313,7 @@ function contact_gap_and_normal(x::AbstractVector,halvspaces::Vector{<:HalfSpace
 end
 
 function contact_gap_and_normal(x::AbstractVector,contact::RigidSphere,bodies)
-    (;bid) = contact
+    (;bid) = contact_body
     foreach(bodies) do body
         if body.prop.id == bid
             radius = body.prop.loci[1].position[1]
@@ -325,101 +325,121 @@ function contact_gap_and_normal(x::AbstractVector,contact::RigidSphere,bodies)
     end
 end
 
+function contact_gap_and_normal!(
+        gaps, normals, local_positions, contact_ids,
+        x::AbstractVector, i, contact_body::RigidSphere, bodies, 
+    )
+    (; bid) = contact_body
+    foreach(bodies) do body
+        if (body.prop.id == bid)
+            (;X) = body.state.origin_frame.axes
+            r = body.state.origin_frame.position
+            radius = body.prop.loci[1].position[1]
+            gap = norm(x - r) - radius
+            normal = (x - r) / norm(x - r)
+            gaps[i] = gap
+            normals[i] = normal
+            contact_ids[i] = i
+            local_positions[i] = X\(x - r)
+        end
+    end
+end
+
+function contact_gap_and_normal!(
+        gaps, normals, local_positions, contact_ids,
+        x::AbstractVector, i, contact_body::RigidCylinder, bodies, 
+    )
+    (; bid) = contact_body
+    foreach(bodies) do body
+        if (body.prop.id == bid)
+            (;X) = body.state.origin_frame.axes
+            rc = body.state.origin_frame.position
+            p = x - rc
+            p̄ = X\p
+            px, py, pz = p̄
+            radius = body.prop.loci[1].position[1]
+            halflen = body.prop.loci[1].position[2]
+            if abs(px) > halflen
+                gap = 1.0
+                normal = SVector(1.0, 0.0, 0.0)
+            elseif norm(SVector(py, pz)) > radius
+                gap = 1.0
+                normal = SVector(1.0, 0.0, 0.0)
+            else
+                gap = norm(SVector(py, pz)) - radius
+                normal = X*SVector(0.0, py, pz) 
+            end
+            if gap < 0 
+                ## @show gap
+            end
+            gaps[i] = gap
+            normals[i] = normal / norm(normal)
+            contact_ids[i] = i
+            local_positions[i] = p̄
+        end
+    end
+end
+
+function contact_gap_and_normal!(
+        gaps, normals, local_positions, contact_ids,
+        x::AbstractVector, i, contact_body::RigidCube, bodies, 
+    )
+    (; bid) = contact_body
+    foreach(bodies) do body
+        if (body.prop.id == bid)
+            (;X) = body.state.origin_frame.axes
+            rc = body.state.origin_frame.position
+            p = x - rc
+            p̄ = X\p
+            px, py, pz = p̄
+            hx, hy, hz= body.prop.loci[1].position
+            if (abs(px) > hx) || (abs(py) > hy) || (abs(pz) > hz)
+                gap = 1.0
+                normal = SVector(1.0, 0.0, 0.0)
+            else
+                normal_vector = [SVector(1.0, 0.0, 0.0),
+                                    SVector(-1.0, 0.0, 0.0),
+                                    SVector(0.0, 1.0, 0.0),
+                                    SVector(0.0, -1.0, 0.0),
+                                    SVector(0.0, 0.0, 1.0),
+                                    SVector(0.0, 0.0, -1.0)]
+                gap_vector = [hx-px, px+hx, hy-py, py+hy, hz-pz, pz+hz]
+                min_idx = argmin(gap_vector)
+                gap = - gap_vector[min_idx]
+                normal = X*normal_vector[min_idx]
+                # @show px, py, pz
+                # @show gap
+            end
+            gaps[i] = gap
+            normals[i] = normal
+            contact_ids[i] = i
+            local_positions[i] = p̄
+        end
+    end
+end
+
 function contact_gap_and_normal(x::AbstractVector, contact_bodies::Vector{<:ContactRigid}, bodies)
     ncb = length(contact_bodies)
     gaps = zeros(Float64, ncb)
     normals = Vector{SVector{3,Float64}}(undef, ncb)
-    local_poss = Vector{SVector{3,Float64}}(undef, ncb)
+    local_positions = Vector{SVector{3,Float64}}(undef, ncb)
     contact_ids = Vector{Int}(undef, ncb)
-    for (i, contact) in enumerate(contact_bodies)
-        if isa(contact, RigidSphere)
-            (; bid) = contact
-            foreach(bodies) do body
-                if (body.prop.id == bid)
-                    (;X) = body.state.origin_frame.axes
-                    r = body.state.origin_frame.position
-                    radius = body.prop.loci[1].position[1]
-                    gap = norm(x - r) - radius
-                    normal = (x - r) / norm(x - r)
-                    gaps[i] = gap
-                    normals[i] = normal
-                    contact_ids[i] = i
-                    local_poss[i] = inv(X)*(x - r)
-                end
-            end
-        elseif isa(contact, RigidCylinder)
-            (; bid) = contact
-            foreach(bodies) do body
-                if (body.prop.id == bid)
-                    (;X) = body.state.origin_frame.axes
-                    rc = body.state.origin_frame.position
-                    p = x - rc
-                    px, py, pz = inv(X) * p
-                    radius = body.prop.loci[1].position[1]
-                    halflen = body.prop.loci[1].position[2]
-                    if abs(px) > halflen
-                        gap = 1.0
-                        normal = SVector(1.0, 0.0, 0.0)
-                    elseif norm(SVector(py, pz)) > radius
-                        gap = 1.0
-                        normal = SVector(1.0, 0.0, 0.0)
-                    else
-                        gap = norm(SVector(py, pz)) - radius
-                        normal = X*SVector(0.0, py, pz) 
-                    end
-                    if gap < 0 
-                        @show gap
-                    end
-                    gaps[i] = gap
-                    normals[i] = normal / norm(normal)
-                    contact_ids[i] = i
-                    local_poss[i] = SVector(px, py, pz)
-                end
-            end
-        elseif isa(contact, RigidCube)
-            (; bid) = contact
-            foreach(bodies) do body
-                if (body.prop.id == bid)
-                    (;X) = body.state.origin_frame.axes
-                    rc = body.state.origin_frame.position
-                    p = x - rc
-                    px, py, pz = inv(X) * p
-                    hx, hy, hz= body.prop.loci[1].position
-                    if (abs(px) > hx) || (abs(py) > hy) || (abs(pz) > hz)
-                        gap = 1.0
-                        normal = SVector(1.0, 0.0, 0.0)
-                    else
-                        normal_vector = [SVector(1.0, 0.0, 0.0),
-                                         SVector(-1.0, 0.0, 0.0),
-                                         SVector(0.0, 1.0, 0.0),
-                                         SVector(0.0, -1.0, 0.0),
-                                         SVector(0.0, 0.0, 1.0),
-                                         SVector(0.0, 0.0, -1.0)]
-                        gap_vector = [hx-px, px+hx, hy-py, py+hy, hz-pz, pz+hz]
-                        min_idx = argmin(gap_vector)
-                        gap = - gap_vector[min_idx]
-                        normal = X*normal_vector[min_idx]
-                        # @show px, py, pz
-                        # @show gap
-                    end
-                    gaps[i] = gap
-                    normals[i] = normal
-                    contact_ids[i] = i
-                    local_poss[i] = SVector(px, py, pz)
-                end
-            end
-        end
+    for (i, contact_body) in enumerate(contact_bodies)
+        contact_gap_and_normal!(
+            gaps, normals, local_positions, contact_ids,
+            x, i,contact_body, bodies, 
+        )
     end
     if all(gaps .< 0)
         icb = argmax(gaps)
         contact_bodies[icb].pid += 1
-        return gaps[icb], normals[icb], contact_ids[icb], local_poss[icb]
+        return gaps[icb], normals[icb], contact_ids[icb], local_positions[icb]
     else
         idx = findall((x) -> x >= 0, gaps)
         positive_gaps = @view gaps[idx]
         positive_normals = @view normals[idx]
         positive_contact_body_ids = @view contact_ids[idx]
         icb = argmin(positive_gaps)
-        return positive_gaps[icb], positive_normals[icb], positive_contact_body_ids[icb], local_poss[icb]
+        return positive_gaps[icb], positive_normals[icb], positive_contact_body_ids[icb], local_positions[icb]
     end
 end
diff --git a/src/mechanics/dynamics.jl b/src/mechanics/dynamics.jl
@@ -103,21 +103,7 @@ function generalized_force_jacobian!(∂F∂q̌,∂F∂q̌̇,bot::Robot,policy::
     build_tangent_damping_matrix!(∂F∂q̌̇,structure)
 end
 
-function generalized_force_jacobian!(∂F∂q̌,∂F∂q̌̇,bot::Robot,policy::AbstractPolicy,q,q̇,t,∂F∂s,cluster_cache,s=nothing)
-    (;structure,hub) = bot
-    ∂F∂q̌ .= 0
-    ∂F∂q̌̇ .= 0
-    ∂F∂s .= 0
-    clear_forces!(structure)
-    lazy_update_bodies!(structure,q,q̇)
-    update_apparatuses!(structure, s)
-    generalized_force_jacobian!(∂F∂s, cluster_cache, structure)
-    generalized_force_jacobian!(∂F∂q̌,∂F∂q̌̇,structure,hub,policy,q,q̇,t)
-    build_tangent_stiffness_matrix!(∂F∂q̌,structure)
-    build_tangent_damping_matrix!(∂F∂q̌̇,structure)
-end
-
-function generalized_force_jacobian!(∂F∂s,structure,q,q̇,t,s,)
+function generalized_force_jacobian!(∂F∂s,structure::AbstractStructure,q,q̇,t,s)
     (;apparatuses,connectivity) = structure
     (;
         apparid2sys_add_var_idx,
@@ -127,7 +113,7 @@ function generalized_force_jacobian!(∂F∂s,structure,q,q̇,t,s,)
         appar_add_var_idx = apparid2sys_add_var_idx[appar.id]
         appar_free_coords_idx = apparid2sys_free_coords_idx[appar.id]
         generalized_force_jacobian!(
-            (@view ∂F∂s[appar_free_coords_idx,appar_add_var_idx]),
+            (@view ∂F∂s[unique(appar_free_coords_idx),appar_add_var_idx]),
             appar,
             structure,
             q,q̇,t,

diff --git a/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_CCP_constant_mass.jl b/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_CCP_constant_mass.jl
@@ -192,7 +192,7 @@ function solve!(sim::Simulator,solver_cache::Zhong06_CCP_Constant_Mass_Cache;
     λ0 = traj.λ[begin]
     s0 = traj.s[begin]
     q̇0 = traj.q̇[begin]
-    activate_contacts!(structure,env,solver_cache,q0)
+    activate_frictional_contacts!(structure,env,solver_cache,q0)
     invM = inv(M)
     pₖ₋₁ = M*q̇0
     pₖ   = zero(pₖ₋₁)