diff --git "a/yard/EWei/STL/\346\210\252\351\235\242\345\275\242\347\212\266-1.STL" "b/assets/EWei_STL/\346\210\252\351\235\242\345\275\242\347\212\266-1.STL"
similarity index 100%
rename from "yard/EWei/STL/\346\210\252\351\235\242\345\275\242\347\212\266-1.STL"
rename to "assets/EWei_STL/\346\210\252\351\235\242\345\275\242\347\212\266-1.STL"
diff --git "a/yard/EWei/STL/\346\210\252\351\235\242\345\275\242\347\212\266-10-\351\232\217\345\212\250\346\256\265.STL" "b/assets/EWei_STL/\346\210\252\351\235\242\345\275\242\347\212\266-10-\351\232\217\345\212\250\346\256\265.STL"
similarity index 100%
rename from "yard/EWei/STL/\346\210\252\351\235\242\345\275\242\347\212\266-10-\351\232\217\345\212\250\346\256\265.STL"
rename to "assets/EWei_STL/\346\210\252\351\235\242\345\275\242\347\212\266-10-\351\232\217\345\212\250\346\256\265.STL"
diff --git "a/assets/EWei_STL/\346\210\252\351\235\242\345\275\242\347\212\266-2.STL" "b/assets/EWei_STL/\346\210\252\351\235\242\345\275\242\347\212\266-2.STL"
new file mode 100644
index 0000000..79ebb1b
Binary files /dev/null and "b/assets/EWei_STL/\346\210\252\351\235\242\345\275\242\347\212\266-2.STL" differ
diff --git "a/yard/EWei/STL/\346\210\252\351\235\242\345\275\242\347\212\266-3.STL" "b/assets/EWei_STL/\346\210\252\351\235\242\345\275\242\347\212\266-3.STL"
similarity index 100%
rename from "yard/EWei/STL/\346\210\252\351\235\242\345\275\242\347\212\266-3.STL"
rename to "assets/EWei_STL/\346\210\252\351\235\242\345\275\242\347\212\266-3.STL"
diff --git "a/yard/EWei/STL/\346\210\252\351\235\242\345\275\242\347\212\266-4-\351\232\217\345\212\250\346\256\265.STL" "b/assets/EWei_STL/\346\210\252\351\235\242\345\275\242\347\212\266-4-\351\232\217\345\212\250\346\256\265.STL"
similarity index 100%
rename from "yard/EWei/STL/\346\210\252\351\235\242\345\275\242\347\212\266-4-\351\232\217\345\212\250\346\256\265.STL"
rename to "assets/EWei_STL/\346\210\252\351\235\242\345\275\242\347\212\266-4-\351\232\217\345\212\250\346\256\265.STL"
diff --git "a/assets/EWei_STL/\346\210\252\351\235\242\345\275\242\347\212\266-5-\351\232\217\345\212\250\346\256\265.STL" "b/assets/EWei_STL/\346\210\252\351\235\242\345\275\242\347\212\266-5-\351\232\217\345\212\250\346\256\265.STL"
new file mode 100644
index 0000000..abad2e1
Binary files /dev/null and "b/assets/EWei_STL/\346\210\252\351\235\242\345\275\242\347\212\266-5-\351\232\217\345\212\250\346\256\265.STL" differ
diff --git "a/yard/EWei/STL/\346\210\252\351\235\242\345\275\242\347\212\266-6-\351\232\217\345\212\250\346\256\265.STL" "b/assets/EWei_STL/\346\210\252\351\235\242\345\275\242\347\212\266-6-\351\232\217\345\212\250\346\256\265.STL"
similarity index 100%
rename from "yard/EWei/STL/\346\210\252\351\235\242\345\275\242\347\212\266-6-\351\232\217\345\212\250\346\256\265.STL"
rename to "assets/EWei_STL/\346\210\252\351\235\242\345\275\242\347\212\266-6-\351\232\217\345\212\250\346\256\265.STL"
diff --git "a/yard/EWei/STL/\346\210\252\351\235\242\345\275\242\347\212\266-7-\351\232\217\345\212\250\346\256\265.STL" "b/assets/EWei_STL/\346\210\252\351\235\242\345\275\242\347\212\266-7-\351\232\217\345\212\250\346\256\265.STL"
similarity index 100%
rename from "yard/EWei/STL/\346\210\252\351\235\242\345\275\242\347\212\266-7-\351\232\217\345\212\250\346\256\265.STL"
rename to "assets/EWei_STL/\346\210\252\351\235\242\345\275\242\347\212\266-7-\351\232\217\345\212\250\346\256\265.STL"
diff --git "a/yard/EWei/STL/\346\210\252\351\235\242\345\275\242\347\212\266-8-\351\232\217\345\212\250\346\256\265.STL" "b/assets/EWei_STL/\346\210\252\351\235\242\345\275\242\347\212\266-8-\351\232\217\345\212\250\346\256\265.STL"
similarity index 100%
rename from "yard/EWei/STL/\346\210\252\351\235\242\345\275\242\347\212\266-8-\351\232\217\345\212\250\346\256\265.STL"
rename to "assets/EWei_STL/\346\210\252\351\235\242\345\275\242\347\212\266-8-\351\232\217\345\212\250\346\256\265.STL"
diff --git "a/assets/EWei_STL/\346\210\252\351\235\242\345\275\242\347\212\266-9-\351\232\217\345\212\250\346\256\265.STL" "b/assets/EWei_STL/\346\210\252\351\235\242\345\275\242\347\212\266-9-\351\232\217\345\212\250\346\256\265.STL"
new file mode 100644
index 0000000..844d12c
Binary files /dev/null and "b/assets/EWei_STL/\346\210\252\351\235\242\345\275\242\347\212\266-9-\351\232\217\345\212\250\346\256\265.STL" differ
diff --git "a/assets/EWei_STL/\350\243\205\351\205\215\344\275\223-\344\270\255\351\227\264\346\242\2016.0-32.STL" "b/assets/EWei_STL/\350\243\205\351\205\215\344\275\223-\344\270\255\351\227\264\346\242\2016.0-32.STL"
new file mode 100644
index 0000000..a8dc2de
Binary files /dev/null and "b/assets/EWei_STL/\350\243\205\351\205\215\344\275\223-\344\270\255\351\227\264\346\242\2016.0-32.STL" differ
diff --git "a/assets/EWei_STL/\350\243\205\351\205\215\344\275\223-\344\270\255\351\227\264\346\242\2016.0-40.STL" "b/assets/EWei_STL/\350\243\205\351\205\215\344\275\223-\344\270\255\351\227\264\346\242\2016.0-40.STL"
new file mode 100644
index 0000000..159b370
Binary files /dev/null and "b/assets/EWei_STL/\350\243\205\351\205\215\344\275\223-\344\270\255\351\227\264\346\242\2016.0-40.STL" differ
diff --git "a/yard/EWei/STL/\350\243\205\351\205\215\344\275\223-\344\270\255\351\227\264\351\203\250\345\210\206.STL" "b/assets/EWei_STL/\350\243\205\351\205\215\344\275\223-\344\270\255\351\227\264\351\203\250\345\210\206.STL"
similarity index 100%
rename from "yard/EWei/STL/\350\243\205\351\205\215\344\275\223-\344\270\255\351\227\264\351\203\250\345\210\206.STL"
rename to "assets/EWei_STL/\350\243\205\351\205\215\344\275\223-\344\270\255\351\227\264\351\203\250\345\210\206.STL"
diff --git "a/yard/EWei/STL/\350\243\205\351\205\215\344\275\223-\344\270\273\345\212\250-\351\232\217\345\212\250.STL" "b/assets/EWei_STL/\350\243\205\351\205\215\344\275\223-\344\270\273\345\212\250-\351\232\217\345\212\250.STL"
similarity index 100%
rename from "yard/EWei/STL/\350\243\205\351\205\215\344\275\223-\344\270\273\345\212\250-\351\232\217\345\212\250.STL"
rename to "assets/EWei_STL/\350\243\205\351\205\215\344\275\223-\344\270\273\345\212\250-\351\232\217\345\212\250.STL"
diff --git "a/assets/EWei_STL/\350\243\205\351\205\215\344\275\223-\347\224\265\346\234\272.STL" "b/assets/EWei_STL/\350\243\205\351\205\215\344\275\223-\347\224\265\346\234\272.STL"
new file mode 100644
index 0000000..3b4d8ba
Binary files /dev/null and "b/assets/EWei_STL/\350\243\205\351\205\215\344\275\223-\347\224\265\346\234\272.STL" differ
diff --git "a/yard/tail/\345\260\276\351\263\215-v1 - \347\241\254.STL" "b/assets/tail/\345\260\276\351\263\215-v1 - \347\241\254.STL"
similarity index 100%
rename from "yard/tail/\345\260\276\351\263\215-v1 - \347\241\254.STL"
rename to "assets/tail/\345\260\276\351\263\215-v1 - \347\241\254.STL"
diff --git "a/yard/tail/\345\260\276\351\263\215-v1-\350\275\257.STL" "b/assets/tail/\345\260\276\351\263\215-v1-\350\275\257.STL"
similarity index 100%
rename from "yard/tail/\345\260\276\351\263\215-v1-\350\275\257.STL"
rename to "assets/tail/\345\260\276\351\263\215-v1-\350\275\257.STL"
diff --git "a/yard/tail/\346\221\206\346\235\206-v1.2.2.STL" "b/assets/tail/\346\221\206\346\235\206-v1.2.2.STL"
similarity index 100%
rename from "yard/tail/\346\221\206\346\235\206-v1.2.2.STL"
rename to "assets/tail/\346\221\206\346\235\206-v1.2.2.STL"
diff --git "a/yard/tail/\346\226\260\345\273\272\346\226\207\346\234\254\346\226\207\346\241\243.txt" "b/assets/tail/\346\226\260\345\273\272\346\226\207\346\234\254\346\226\207\346\241\243.txt"
similarity index 100%
rename from "yard/tail/\346\226\260\345\273\272\346\226\207\346\234\254\346\226\207\346\241\243.txt"
rename to "assets/tail/\346\226\260\345\273\272\346\226\207\346\234\254\346\226\207\346\241\243.txt"
diff --git "a/yard/tail/\350\202\213\347\211\207-3.STL" "b/assets/tail/\350\202\213\347\211\207-3.STL"
similarity index 100%
rename from "yard/tail/\350\202\213\347\211\207-3.STL"
rename to "assets/tail/\350\202\213\347\211\207-3.STL"
diff --git "a/yard/tail/\350\202\213\347\211\207-4.STL" "b/assets/tail/\350\202\213\347\211\207-4.STL"
similarity index 100%
rename from "yard/tail/\350\202\213\347\211\207-4.STL"
rename to "assets/tail/\350\202\213\347\211\207-4.STL"
diff --git "a/yard/tail/\350\202\213\347\211\207-5 - \345\211\257\346\234\254.STL" "b/assets/tail/\350\202\213\347\211\207-5 - \345\211\257\346\234\254.STL"
similarity index 100%
rename from "yard/tail/\350\202\213\347\211\207-5 - \345\211\257\346\234\254.STL"
rename to "assets/tail/\350\202\213\347\211\207-5 - \345\211\257\346\234\254.STL"
diff --git "a/yard/tail/\350\202\213\347\211\207-7 - \345\211\257\346\234\254.STL" "b/assets/tail/\350\202\213\347\211\207-7 - \345\211\257\346\234\254.STL"
similarity index 100%
rename from "yard/tail/\350\202\213\347\211\207-7 - \345\211\257\346\234\254.STL"
rename to "assets/tail/\350\202\213\347\211\207-7 - \345\211\257\346\234\254.STL"
diff --git "a/yard/tail/\350\202\213\347\211\207-7 - \347\241\254\350\277\236\346\216\245.STL" "b/assets/tail/\350\202\213\347\211\207-7 - \347\241\254\350\277\236\346\216\245.STL"
similarity index 100%
rename from "yard/tail/\350\202\213\347\211\207-7 - \347\241\254\350\277\236\346\216\245.STL"
rename to "assets/tail/\350\202\213\347\211\207-7 - \347\241\254\350\277\236\346\216\245.STL"
diff --git "a/yard/tail/\350\243\205\351\205\215\344\275\223\345\244\264\351\203\250-1.2.3.STL" "b/assets/tail/\350\243\205\351\205\215\344\275\223\345\244\264\351\203\250-1.2.3.STL"
similarity index 100%
rename from "yard/tail/\350\243\205\351\205\215\344\275\223\345\244\264\351\203\250-1.2.3.STL"
rename to "assets/tail/\350\243\205\351\205\215\344\275\223\345\244\264\351\203\250-1.2.3.STL"
diff --git a/examples/bodies/build_2d_tri.jl b/examples/bodies/build_2d_tri.jl
index d2243c5..2b6b13e 100644
--- a/examples/bodies/build_2d_tri.jl
+++ b/examples/bodies/build_2d_tri.jl
@@ -70,9 +70,9 @@ state = RB.RigidBodyState(prop,ro,α,ṙo,ω)
 coords = RB.NonminimalCoordinates(nmcs,ci,cstr_idx)
 trimesh = begin
     if id == 1
-        load(RB.assetpath("零件1 - 副本.STL")) |> make_patch(;scale=1/1000,color=:mediumpurple4)
+        load(RB.assetpath("零件1 - 副本.STL")) |> RB.make_patch(;scale=1/1000,color=:mediumpurple4)
     else
-        load(RB.assetpath("零件1.STL")) |> make_patch(;scale=1/1000,color=:slategray4)
+        load(RB.assetpath("零件1.STL")) |> RB.make_patch(;scale=1/1000,color=:slategray4)
     end
 end
 body = RB.RigidBody(prop,state,coords,trimesh)
diff --git a/examples/bodies/make_3d_tri.jl b/examples/bodies/make_3d_tri.jl
index 6335306..cab66d4 100644
--- a/examples/bodies/make_3d_tri.jl
+++ b/examples/bodies/make_3d_tri.jl
@@ -92,7 +92,7 @@ if loadmesh
 else
     trimesh = GB.merge(
         [
-            endpoints2mesh(loci_positions[i],loci_positions[j];
+            RB.endpoints2mesh(loci_positions[i],loci_positions[j];
             radius,color)
             for (i,j) in [
                 [1,2],[1,3],[1,4],
diff --git a/yard/EWei/define.jl b/examples/robots/EWei.jl
similarity index 81%
rename from yard/EWei/define.jl
rename to examples/robots/EWei.jl
index a2832e7..c4acce8 100644
--- a/yard/EWei/define.jl
+++ b/examples/robots/EWei.jl
@@ -40,7 +40,7 @@ function BuildTail()
     function Get_apsAB(r̄, mass_locus)
         ri = r̄
         rj = r̄ + 2* [mass_locus[2], mass_locus[1]]
-        aps = [[0.0, 0.0], 2r̄g, [15.1, 0.0]]
+        aps = [[0.0, 0.0], 2r̄, [15.1, 0.0]]
         return ri, rj, aps
     end
 
@@ -161,7 +161,8 @@ function BuildTail()
         α = atan(u[2], u[1])
         mass_locus = rdsi.mass_locus
         m = rdsi.m*1e-3
-        I = rdsi.i
+        ## I = rdsi.i
+        @show m
         nrp = length(aps)
         ṙo = zeros(2); ω = 0.0
         prop = RB.RigidBodyProperty(
@@ -169,8 +170,8 @@ function BuildTail()
             contactable,
             m,
             SMatrix{2, 2}([
-                I 0
-                0 0
+                m 0
+                0 m
             ]),
             SVector{2}(mass_locus),
             [SVector{2}(aps[i]) for i in 1:nrp],
@@ -178,34 +179,20 @@ function BuildTail()
         )
         ro = SVector{2}(ri)
         if i in rigidIndex
-            nmcs = RB.NCF.NC2P1V(SVector{2}(ri), SVector{2}(rj), ro, α)
+            @show i, "NC2P1V", ri, rj
+            nmcs = RB.NCF.NC2P1V(SVector{2}(ri), SVector{2}(rj), ro, RB.rotation_matrix(α))
         else
-            nmcs = RB.NCF.NC2D2P(SVector{2}(ri), SVector{2}(rj), ro, α)
+            @show i, "NC2D2P"
+            nmcs = RB.NCF.NC2D2P(SVector{2}(ri), SVector{2}(rj), ro, RB.rotation_matrix(α))
         end
-        state = RB.RigidBodyState(prop, nmcs, ri, α, ṙo, ω, ci, cstr_idx)
-        body = RB.RigidBody(prop, state)
+        state = RB.RigidBodyState(prop, ri, α, ṙo, ω, )
+        coords = RB.NonminimalCoordinates(nmcs, ci, cstr_idx)
+        body = RB.RigidBody(prop, state,coords)
     end
 
     rbs = [rigidbody(i, rds[i], r̄s[i]) for i in 1:nb]
     rigdibodies = TypeSortedCollection(rbs)
-    numbered = RB.number(rigdibodies)
-    matrix_sharing_raw = Vector{Matrix{Int}}()
-    for i in 1:nb-1
-        s = zeros(2, nb)
-        if i == 1
-            s[1:2, 1] = 3:4
-            s[1:2, 2] = 1:2
-        elseif i in 2:2:16
-            s[1:2, i] = 1:2
-            s[1:2, i+1] = 1:2
-        else
-            s[1:2, i] = 3:4
-            s[1:2, i+1] = 1:2
-        end
-        push!(matrix_sharing_raw, s)
-    end
-    matrix_sharing = reduce(vcat, matrix_sharing_raw)
-    indexed = RB.index(rigdibodies, matrix_sharing)
+
     nstrings = 8 * 4 + 8 * 2
     ks = zeros(nstrings); restlens = zeros(nstrings)
     for i in 1:4*8
@@ -217,8 +204,7 @@ function BuildTail()
         restlens[i] = 25
         ks[i] = 0.3
     end
-    cables = [RB.DistanceSpringDamper2D( restlens[i], ks[i], 0.0) for i in 1:nstrings]
-    apparatuses = (cables=cables,)
+    spring_dampers = [RB.DistanceSpringDamper2D( restlens[i], ks[i], 0.0) for i in 1:nstrings]
 
     matrix_cnt_raw = Vector{Matrix{Int}}()
     s = zeros(4, nb)
@@ -245,11 +231,45 @@ function BuildTail()
         s[2, i] = 2; s[2, i+1] = -2
         push!(matrix_cnt_raw, s)
     end
-    matrix_cnt = reduce(vcat, matrix_cnt_raw)
-    tensiled = (connected=RB.connect(rigdibodies, matrix_cnt),)
-    cnt = RB.Connectivity(numbered, indexed, tensiled)
+    connecting_matrix = reduce(vcat, matrix_cnt_raw)
+    spring_dampers = RB.connect(rigdibodies, spring_dampers;connecting_matrix)
+    apparatuses = TypeSortedCollection(
+        spring_dampers
+    )
+
+    numbered = RB.number(rigdibodies, apparatuses)
+    
+    matrix_sharing_raw = Vector{Matrix{Int}}()
+    for i in 1:nb-1
+        s = zeros(2, nb)
+        if i == 1
+            s[1:2, 1] = 3:4
+            s[1:2, 2] = 1:2
+        elseif i in 2:2:16
+            s[1:2, i] = 1:2
+            s[1:2, i+1] = 1:2
+        else
+            s[1:2, i] = 3:4
+            s[1:2, i+1] = 1:2
+        end
+        push!(matrix_sharing_raw, s)
+    end
+    sharing_matrix = reduce(vcat, matrix_sharing_raw)
+    indexed = RB.index(rigdibodies, apparatuses; sharing_matrix)
+    cnt = RB.Connectivity(
+        numbered, 
+        indexed,
+    )
     st = RB.Structure(rigdibodies, apparatuses, cnt)
-    bot = RB.Robot(st,)
+    gauges = Int[]
+    actuators = Int[]
+    hub = RB.ControlHub(
+        st,
+        gauges,
+        actuators,
+        RB.Coalition(st,gauges,actuators)
+    )
+    bot = RB.Robot(st,hub)
 end
 
 """
@@ -291,7 +311,7 @@ function BuildTail(type; β=1.0, μ=0.02)
     function Get_apsAB(r̄, mass_locus)
         ri = r̄
         rj = r̄ + 2* [mass_locus[2], mass_locus[1]]
-        aps = [[0.0, 0.0], 2r̄g, [15.1, 0.0]]
+        aps = [[0.0, 0.0], 2r̄, [15.1, 0.0]]
         return ri, rj, aps
     end
 
@@ -325,23 +345,23 @@ function BuildTail(type; β=1.0, μ=0.02)
             4 => Get_apsC()
         end
     end
-    asbMid = load("STL/装配体-中间部分.STL") |> make_patch(;trans=[0.0,0,0],rot=RotZ(deg2rad(-77)))
-    crSec1 = load("STL/截面形状-1.STL") |> make_patch(;trans=[0.0,-5,0]) |> (x)->merge([x,asbMid])
-    crSec2 = load("STL/截面形状-2.STL") |> make_patch(;trans=[0.0,-5,0]) |> (x)->merge([x,asbMid])
-    crSec3 = load("STL/截面形状-3.STL") |> make_patch(;trans=[0.0,-5,0]) |> (x)->merge([x,asbMid])
-    crSec4f = load("STL/截面形状-4-随动段.STL") |> make_patch(;trans=[40,0,0.],rot=RotZ(-π/2))
-    crSec5f = load("STL/截面形状-5-随动段.STL") |> make_patch(;trans=[40,0,0.],rot=RotZ(-π/2))
-    crSec6f = load("STL/截面形状-6-随动段.STL") |> make_patch(;trans=[30,0,0.],rot=RotZ(-π/2))
-    crSec7f = load("STL/截面形状-7-随动段.STL") |> make_patch(;trans=[25,0,0.],rot=RotZ(-π/2))
-    crSec8f = load("STL/截面形状-8-随动段.STL") |> make_patch(;trans=[25,0,0.],rot=RotZ(-π/2))
-    crSec9f = load("STL/截面形状-9-随动段.STL") |> make_patch(;trans=[25,0,0.],rot=RotZ(-π/2))
-    crSec10f = load("STL/截面形状-10-随动段.STL") |> make_patch(;trans=[30,0,0.],rot=RotZ(-π/2))
-
-    asbMot = load("STL/装配体-电机.STL") |> make_patch(;trans=[45.0,0,0],rot=RotZ(π))
-
-    asbB32 = load("STL/装配体-中间梁6.0-32.STL") |> make_patch(;trans=[25.0,0,0])
-    asbB40 = load("STL/装配体-中间梁6.0-40.STL") |> make_patch(;trans=[25.0,0,0])
-    asbLF = load("STL/装配体-主动-随动.STL") |> make_patch(;trans=[55,0,0.],rot=RotZ( π/2))
+    asbMid = load(RB.assetpath("EWei_STL/装配体-中间部分.STL")) |> RB.make_patch(;trans=[0.0,0,0],rot=RotZ(deg2rad(-77)))
+    crSec1 = load(RB.assetpath("EWei_STL/截面形状-1.STL")) |> RB.make_patch(;trans=[0.0,-5,0]) |> (x)->merge([x,asbMid])
+    crSec2 = load(RB.assetpath("EWei_STL/截面形状-2.STL")) |> RB.make_patch(;trans=[0.0,-5,0]) |> (x)->merge([x,asbMid])
+    crSec3 = load(RB.assetpath("EWei_STL/截面形状-3.STL")) |> RB.make_patch(;trans=[0.0,-5,0]) |> (x)->merge([x,asbMid])
+    crSec4f = load(RB.assetpath("EWei_STL/截面形状-4-随动段.STL")) |> RB.make_patch(;trans=[40,0,0.],rot=RotZ(-π/2))
+    crSec5f = load(RB.assetpath("EWei_STL/截面形状-5-随动段.STL")) |> RB.make_patch(;trans=[40,0,0.],rot=RotZ(-π/2))
+    crSec6f = load(RB.assetpath("EWei_STL/截面形状-6-随动段.STL")) |> RB.make_patch(;trans=[30,0,0.],rot=RotZ(-π/2))
+    crSec7f = load(RB.assetpath("EWei_STL/截面形状-7-随动段.STL")) |> RB.make_patch(;trans=[25,0,0.],rot=RotZ(-π/2))
+    crSec8f = load(RB.assetpath("EWei_STL/截面形状-8-随动段.STL")) |> RB.make_patch(;trans=[25,0,0.],rot=RotZ(-π/2))
+    crSec9f = load(RB.assetpath("EWei_STL/截面形状-9-随动段.STL")) |> RB.make_patch(;trans=[25,0,0.],rot=RotZ(-π/2))
+    crSec10f = load(RB.assetpath("EWei_STL/截面形状-10-随动段.STL")) |> RB.make_patch(;trans=[30,0,0.],rot=RotZ(-π/2))
+
+    asbMot = load(RB.assetpath("EWei_STL/装配体-电机.STL")) |> RB.make_patch(;trans=[45.0,0,0],rot=RotZ(π))
+
+    asbB32 = load(RB.assetpath("EWei_STL/装配体-中间梁6.0-32.STL")) |> RB.make_patch(;trans=[25.0,0,0])
+    asbB40 = load(RB.assetpath("EWei_STL/装配体-中间梁6.0-40.STL")) |> RB.make_patch(;trans=[25.0,0,0])
+    asbLF = load(RB.assetpath("EWei_STL/装配体-主动-随动.STL")) |> RB.make_patch(;trans=[55,0,0.],rot=RotZ( π/2))
 
     rds = [
         RigidData(r̄gC, mC, iC, r̄lC, asbMot), #1
@@ -447,34 +467,18 @@ function BuildTail(type; β=1.0, μ=0.02)
         )
         ro = SVector{2}(ri)
         if i in rigidIndex
-            nmcs = RB.NCF.NC2P1V(SVector{2}(ri), SVector{2}(rj), ro, α)
+            nmcs = RB.NCF.NC2P1V(SVector{2}(ri), SVector{2}(rj), ro, RB.rotation_matrix(α))
         else
-            nmcs = RB.NCF.NC2D2P(SVector{2}(ri), SVector{2}(rj), ro, α)
+            nmcs = RB.NCF.NC2D2P(SVector{2}(ri), SVector{2}(rj), ro, RB.rotation_matrix(α))
         end
-        state = RB.RigidBodyState(prop, nmcs, ri, α, ṙo, ω, ci, cstr_idx)
-        body = RB.RigidBody(prop, state, rdsi.mesh)
+        state = RB.RigidBodyState(prop, ri, α, ṙo, ω, )
+        coords = RB.NonminimalCoordinates(nmcs, ci, cstr_idx)
+        body = RB.RigidBody(prop, state, coords, rdsi.mesh)
     end
 
     rbs = [rigidbody(i, rds[i], r̄s[i]) for i in 1:nb]
     rigdibodies = TypeSortedCollection(rbs)
-    numbered = RB.number(rigdibodies)
-    matrix_sharing_raw = Vector{Matrix{Int}}()
-    for i in 1:nb-1
-        s = zeros(2, nb)
-        if i == 1
-            s[1:2, 1] = 3:4
-            s[1:2, 2] = 1:2
-        elseif i in 2:2:16
-            s[1:2, i] = 1:2
-            s[1:2, i+1] = 1:2
-        else
-            s[1:2, i] = 3:4
-            s[1:2, i+1] = 1:2
-        end
-        push!(matrix_sharing_raw, s)
-    end
-    matrix_sharing = reduce(vcat, matrix_sharing_raw)
-    indexed = RB.index(rigdibodies, matrix_sharing)
+
     nstrings = 8 * 4 + 8 * 2
     ks = zeros(nstrings); restlens = zeros(nstrings)
     for i in 1:4*8
@@ -486,7 +490,7 @@ function BuildTail(type; β=1.0, μ=0.02)
         restlens[i] = 25
         ks[i] = 0.3
     end
-    cables = [RB.DistanceSpringDamper2D( restlens[i], ks[i], 0.0) for i in 1:nstrings]
+    spring_dampers = [RB.DistanceSpringDamper2D( restlens[i], ks[i], 0.0) for i in 1:nstrings]
     @match type begin
         1 => begin
             lens = [44.71, 50, 50]
@@ -494,7 +498,7 @@ function BuildTail(type; β=1.0, μ=0.02)
             c_section = StructArray([RB.DistanceSpringDamperSegment(i, lens[i], 0.2, prestress=pre[i]) for i in 1:3])
             cs1 = RB.ClusterDistanceSpringDampers(1, 2, deepcopy(c_section); μ=μ)
             cs2 = RB.ClusterDistanceSpringDampers(2, 2, deepcopy(c_section); μ=μ)
-            apparatuses = (cables=cables, clustercables=[cs1, cs2])
+            apparatuses = (spring_dampers=spring_dampers, clustercables=[cs1, cs2])
             s = zeros(Int, 2, nb)
             s[1, 1] = 3; s[1, 2] = 2; s[1, 4] = 2; s[1, 6] = 2
             s[2, 1] = 2; s[2, 2] = 1; s[2, 4] = 1; s[2, 6] = 1
@@ -506,7 +510,7 @@ function BuildTail(type; β=1.0, μ=0.02)
             c_section = StructArray([RB.DistanceSpringDamperSegment(i, lens[i], 0.2, prestress=pre[i]) for i in 1:8])
             cs1 = RB.ClusterDistanceSpringDampers(1, 7, deepcopy(c_section); μ=μ)
             cs2 = RB.ClusterDistanceSpringDampers(2, 7, deepcopy(c_section); μ=μ)
-            apparatuses = (cables=cables, clustercables=[cs1, cs2])
+            apparatuses = (spring_dampers=spring_dampers, clustercables=[cs1, cs2])
             s = zeros(Int, 2, nb)
             s[1, 1] = 3; s[2, 1] = 2
             for i in 1:8
@@ -523,7 +527,7 @@ function BuildTail(type; β=1.0, μ=0.02)
             c_section2 = StructArray([RB.DistanceSpringDamperSegment(i, lens2[i], 0.2, prestress=pre2[i]) for i in 1:2])
             cs1 = [RB.ClusterDistanceSpringDampers(i, 1, deepcopy(c_section1); μ=μ) for i in 1:2]
             cs2 = [RB.ClusterDistanceSpringDampers(i, 1, deepcopy(c_section2); μ=μ) for i in 3:8]
-            apparatuses = (cables=cables, clustercables=vcat(cs1, cs2))
+            apparatuses = (spring_dampers=spring_dampers, clustercables=vcat(cs1, cs2))
             s = zeros(Int, 8, nb)
             s[1, 1] = 3; s[1, 2] = 2; s[1, 4] = 2;
             s[2, 1] = 2; s[2, 2] = 1; s[2, 4] = 1;
@@ -543,7 +547,7 @@ function BuildTail(type; β=1.0, μ=0.02)
             c_section2 = StructArray([RB.DistanceSpringDamperSegment(i, lens2[i], 0.2, prestress=pre2[i]) for i in 1:3])
             cs1 = [RB.ClusterDistanceSpringDampers(i, 2, deepcopy(c_section1); μ=μ) for i in 1:2]
             cs2 = [RB.ClusterDistanceSpringDampers(i, 2, deepcopy(c_section2); μ=μ) for i in 3:6]
-            apparatuses = (cables=cables, clustercables=vcat(cs1, cs2))
+            apparatuses = (spring_dampers=spring_dampers, clustercables=vcat(cs1, cs2))
             s = zeros(Int, 6, nb)
             s[1, 1] = 3; s[1, 2] = 2; s[1, 4] = 2; s[1, 6] = 2
             s[2, 1] = 2; s[2, 2] = 1; s[2, 4] = 1; s[2, 6] = 1
@@ -584,9 +588,42 @@ function BuildTail(type; β=1.0, μ=0.02)
         s[2, i] = 2; s[2, i+1] = -2
         push!(matrix_cnt_raw, s)
     end
-    matrix_cnt = reduce(vcat, matrix_cnt_raw)
-    cc = RB.connect_and_cluster(rigdibodies, matrix_cnt, matrix_cnt2)
-    cnt = RB.Connectivity(numbered, indexed, cc)
+    connecting_matrix = reduce(vcat, matrix_cnt_raw)
+
+    cc = RB.connect_and_cluster(rigdibodies, connecting_matrix, matrix_cnt2)
+
+    matrix_sharing_raw = Vector{Matrix{Int}}()
+    for i in 1:nb-1
+        s = zeros(2, nb)
+        if i == 1
+            s[1:2, 1] = 3:4
+            s[1:2, 2] = 1:2
+        elseif i in 2:2:16
+            s[1:2, i] = 1:2
+            s[1:2, i+1] = 1:2
+        else
+            s[1:2, i] = 3:4
+            s[1:2, i+1] = 1:2
+        end
+        push!(matrix_sharing_raw, s)
+    end
+    matrix_sharing = reduce(vcat, matrix_sharing_raw)
+
+    numbered = RB.number(rigdibodies, apparatuses;)
+    indexed = RB.index(rigdibodies, apparatuses; matrix_sharing)
+
+    cnt = RB.Connectivity(numbered, indexed)
+
     st = RB.Structure(rigdibodies, apparatuses, cnt)
-    bot = RB.Robot(st,)
+
+    gauges = Int[]
+    actuators = Int[]
+    hub = RB.ControlHub(
+        st,
+        gauges,
+        actuators,
+        RB.Coalition(st,gauges,actuators)
+    )
+
+    bot = RB.Robot(st,hub)
 end
diff --git a/examples/robots/Tbars.jl b/examples/robots/Tbars.jl
index 92f063e..a1b8476 100644
--- a/examples/robots/Tbars.jl
+++ b/examples/robots/Tbars.jl
@@ -132,14 +132,6 @@ function Tbars(;θ = 0, coordsType = RB.NCF.NC)
     rbs = [base,slider1,slider2,bar]
     rigdibodies = TypeSortedCollection(rbs)
 
-    j1 = RB.PrismaticJoint(1,RB.Hen2Egg(RB.Signifier(base,5,1),RB.Signifier(slider1,1,1)))
-    j2 = RB.PrismaticJoint(2,RB.Hen2Egg(RB.Signifier(base,5,2),RB.Signifier(slider2,1,1)))
-
-    j3 = RB.PinJoint(3,RB.Hen2Egg(RB.Signifier(bar,1,1),RB.Signifier(slider1,2,1)))
-    j4 = RB.PinJoint(4,RB.Hen2Egg(RB.Signifier(bar,2,1),RB.Signifier(slider2,2,1)))
-
-    js = [j1,j2,j3,j4]
-
     ncables = 4
     original_restlens = zeros(ncables)
     original_restlens = zeros(ncables)
@@ -148,7 +140,7 @@ function Tbars(;θ = 0, coordsType = RB.NCF.NC)
     cs = zeros(ncables)
     spring_dampers = [RB.DistanceSpringDamper3D(original_restlens[i],ks[i],cs[i];slack=false) for i = 1:ncables]
 
-    cm = [
+    connecting_matrix = [
         1 -1  0  0;
         2 -1  0  0;
         # 3  0 -1  0;
@@ -157,10 +149,19 @@ function Tbars(;θ = 0, coordsType = RB.NCF.NC)
         7 -1  0  0;
     ]
 
-    cables = RB.connect(rigdibodies, spring_dampers, cm, 4)
+    cables = RB.connect(rigdibodies, spring_dampers; connecting_matrix,)
+
+
+    j1 = RB.PrismaticJoint(ncables+1,RB.Hen2Egg(RB.Signifier(base,5,1),RB.Signifier(slider1,1,1)))
+    j2 = RB.PrismaticJoint(ncables+2,RB.Hen2Egg(RB.Signifier(base,5,2),RB.Signifier(slider2,1,1)))
+
+    j3 = RB.PinJoint(ncables+3,RB.Hen2Egg(RB.Signifier(bar,1,1),RB.Signifier(slider1,2,1)))
+    j4 = RB.PinJoint(ncables+4,RB.Hen2Egg(RB.Signifier(bar,2,1),RB.Signifier(slider2,2,1)))
+
+    js = [j1,j2,j3,j4]
 
     apparatuses = TypeSortedCollection(
-        vcat(js,cables)
+        vcat(cables,js)
     )
     
     # sm = [
@@ -173,5 +174,14 @@ function Tbars(;θ = 0, coordsType = RB.NCF.NC)
     # ]
     # indexed = RB.index(rigdibodies,sm)
     st = RB.Structure(rigdibodies,apparatuses,)
-    RB.Robot(st,)
+    gauges = Int[]
+    actuators = Int[]
+    hub = RB.ControlHub(
+        st,
+        gauges,
+        actuators,
+        RB.Coalition(st,gauges,actuators)
+    )
+
+    RB.Robot(st,hub)
 end
\ No newline at end of file
diff --git a/examples/robots/bai.jl b/examples/robots/bai.jl
new file mode 100644
index 0000000..499f0b0
--- /dev/null
+++ b/examples/robots/bai.jl
@@ -0,0 +1,180 @@
+
+# 定义一个函数来新建机器人
+function make_bai(meshes,质心,质量,惯量)
+
+    nb = 6
+    ver_lengths_raw = [
+        #  0   43;
+        # 18   18;
+        0    43+18+18-10;
+        18   16;
+        19.1  9;
+        18    8;
+        18    8;
+         0   16.29;
+         0  150
+    ]
+    ver_lengths = sum(ver_lengths_raw,dims=2)
+    hor_lengths = [
+        32.61,
+        26.21,
+        20.64,
+        14.71,
+        13.24,
+        # 13.24,
+         2.5
+    ]
+    nhor = length(hor_lengths)
+
+    θs = zeros(nhor+1)
+    for i = 2:nhor-1
+        θs[i] = π/48*i
+    end
+    P = [
+        zeros(2,)
+        for i = 1:nhor+1
+    ]
+    for i = 2:nhor+1
+        P[i] = P[i-1] + RB.rotation_matrix(θs[i])*[0.0,-ver_lengths[i-1]]
+    end
+
+    function rigidbody(i,P)
+        if i == 1
+            contactable = false
+            visible = true
+            ci = collect(1:6)
+            cstr_idx = Int[]
+        elseif i in [2,nb]
+            contactable = true
+            visible = true
+            ci = collect(1:2)
+            cstr_idx = collect(1:3)
+        else
+            contactable = true
+            visible = true
+            ci = Int[]
+            cstr_idx = collect(1:3)
+        end
+        α = -π/2 + θs[i]
+        if i == 1
+            ri = SVector{2}(0.0,  0.0)
+            rj = SVector{2}(P[i+1] )
+        elseif i < nb
+            ri = SVector{2}(P[i])
+            rj = SVector{2}(P[i+1])
+        else            
+            ri = SVector{2}(0.0,  0.0)
+            rj = SVector{2}(0.0,-150.0)
+        end
+        b = hor_lengths[i]
+        offset_fix = begin
+            if i == 1
+                ret = 43 + 10
+            elseif i == nb
+                ret = 150
+            else
+                ret = 0
+            end
+            ret 
+        end
+        offset = ver_lengths_raw[i,1]
+        ro = ri + SVector{2}(0.0,-offset)
+        mass_locus = SVector{2}(质心[i])
+        m = 质量[i]
+        inertia = 惯量[i]/2
+        Ī = SMatrix{2,2}([
+            inertia 0
+            0 inertia
+        ])
+        r̄p1 = SVector{2}(offset_fix,-b,) 
+        r̄p2 = SVector{2}(offset_fix, b,)
+        r̄p3 = SVector{2}(-offset, 0.0)
+        r̄p4 = SVector{2}(norm(rj-ri)-offset, 0.0)
+        loci = [r̄p1,r̄p2,r̄p3,r̄p4]
+        nr̄p = length(loci)
+        ṙo = zeros(2); ω = 0.0
+        prop = RB.RigidBodyProperty(
+            i,
+            contactable,
+            m,
+            Ī,
+            SVector{2}(mass_locus),
+            loci;
+            visible = visible,
+        )
+        @show i, ri, rj
+        nmcs = RB.NCF.NC2P1V(ri, rj, ro, RB.rotation_matrix(α))
+        state = RB.RigidBodyState(prop, ro, α, ṙo, ω, )
+        coords = RB.NonminimalCoordinates(nmcs, ci, cstr_idx)
+        mesh_rigid = meshes[i]
+        body = RB.RigidBody(prop, state, coords, mesh_rigid)
+    end
+    rbs = [
+        rigidbody(i, P) for i = 1:nb
+    ]
+    rigdibodies = TypeSortedCollection(rbs)
+
+    ncables = 2(nb-1)
+    original_restlens = zeros(ncables)
+    ks = zeros(ncables)
+    for i = 1:ncables
+        original_restlens[i] = 22.0
+        ks[i] = 1000.0
+    end
+    spring_dampers = [RB.DistanceSpringDamper2D( original_restlens[i], ks[i], 0.0;slack=true) for i = 1:ncables]  
+
+    matrix_cnt_raw = Vector{Matrix{Int}}()
+    for i = 1:nb-2
+        s = zeros(2, nb)
+        s[1, i  ] =  1
+        s[1, i+1] = -1
+        s[2, i  ] =  2
+        s[2, i+1] = -2
+        push!(matrix_cnt_raw, s)
+    end
+    connecting_matrix = reduce(vcat, matrix_cnt_raw)
+    # display(connecting_matrix)
+    cables = RB.connect(rigdibodies, spring_dampers; connecting_matrix)
+    apparatuses = TypeSortedCollection(
+        cables
+    )
+
+    numbered = RB.number(rigdibodies,apparatuses)
+    matrix_sharing_raw = Vector{Matrix{Int}}()
+    for i = 1:nb-2
+        s = zeros(2, nb)
+        s[1:2, i] = 3:4
+        s[1:2, i+1] = 1:2
+        push!(matrix_sharing_raw, s)
+    end
+    s = zeros(2, nb)
+    s[1:2,  1] = 1:2
+    s[1:2, nb] = 1:2
+    push!(matrix_sharing_raw, s)
+    sharing_matrix = reduce(vcat, matrix_sharing_raw)
+    # display(sharing_matrix)
+    indexed = RB.index(rigdibodies, apparatuses; sharing_matrix)
+
+    cnt = RB.Connectivity(numbered, indexed,)
+
+    st = RB.Structure(rigdibodies, apparatuses, cnt)
+
+    gauges = Int[]
+    actuators = Int[]
+    #
+    ## acs = [
+    ##     RB.RegisterActuator(1,
+    ##         [1:2(nb-1)],
+    ##         original_restlens,
+    ##     )
+    ## ]
+    
+    hub = RB.ControlHub(
+        st,
+        gauges,
+        actuators,
+        RB.Coalition(st,gauges,actuators)
+    )
+
+    bot = RB.Robot(st, hub)
+end
\ No newline at end of file
diff --git a/examples/robots/cube.jl b/examples/robots/cube.jl
new file mode 100644
index 0000000..92e8f62
--- /dev/null
+++ b/examples/robots/cube.jl
@@ -0,0 +1,65 @@
+function make_cube(origin_position = [0.0,0.0,0.5],
+        R = one(RotMatrix{3}),
+        origin_velocity = [2.0,0.0,0.0],
+        ω = [0.0,0.0,5.0];
+        μ = 0.9,
+        e = 0.0
+    )
+    m = 1.0
+    mass_locus = zeros(3)
+    inertia = SMatrix{3,3}(Matrix(1.0I,3,3))
+    h = 5.0
+    loci = [h.*[x,y,z] for z = [-1,1] for y = [-1,1] for x = [-1,1]]
+    axes = [SVector(1.0,0,0) for _ = 1:length(loci)]
+    friction_coefficients = [μ for _ = 1:length(loci)]
+    restitution_coefficients = [e for _ = 1:length(loci)]
+    pts = Point3.(loci)
+    fcs = GB.QuadFace.([
+        [1,3,4,2],
+        [3,7,8,4],
+        [7,5,6,8],
+        [5,1,2,6],
+        [2,4,8,6],
+        [1,5,7,3]
+    ])
+    nls = GB.normals(pts,fcs)
+    cube_mesh = GB.Mesh(GB.meta(pts,normals=nls),fcs) |> RB.make_patch(;color=:snow)
+    prop = RB.RigidBodyProperty(
+        1,true,m,inertia,
+        mass_locus,loci,axes,
+        friction_coefficients,restitution_coefficients,
+        )
+    ri = copy(origin_position)
+    state = RB.RigidBodyState(prop,origin_position,R,origin_velocity,ω)
+
+    nmcs = RB.NCF.NC1P3V(ri,origin_position,R)
+    coords = RB.NonminimalCoordinates(nmcs,)
+    rb1 = RB.GravitationalRigidBody(prop,state,coords,cube_mesh)
+    rbs = TypeSortedCollection((rb1,))
+
+    apparatuses = Int[]
+    sharing_matrix = zeros(Int,0,0)
+    indexed = RB.index(rbs,apparatuses;sharing_matrix)
+    numbered = RB.number(rbs,apparatuses)
+
+    cnt = RB.Connectivity(numbered,indexed,)
+
+    st = RB.Structure(rbs,apparatuses,cnt,)
+    
+    gauges = Int[]
+    actuators = Int[]
+    ## actuators = [
+    ##     RB.RegisterActuator(
+    ##         1,
+    ##         collect(1:ncables_prism),
+    ##         zeros(ncables_prism)
+    ##     ),
+    ## ]
+    hub = RB.ControlHub(
+        st,
+        gauges,
+        actuators,
+        RB.Coalition(st,gauges,actuators)
+    )
+    bot = RB.Robot(st,hub)
+end
\ No newline at end of file
diff --git a/examples/robots/pointmass.jl b/examples/robots/pointmass.jl
index 1606140..7255608 100644
--- a/examples/robots/pointmass.jl
+++ b/examples/robots/pointmass.jl
@@ -36,5 +36,22 @@ function new_pointmass(;
     numbered = RB.number(rbs,apparatuses)
     cnt = RB.Connectivity(indexed,numbered,)
     st = RB.Structure(rbs,apparatuses,cnt,)
-    bot = RB.Robot(st)
+
+
+    gauges = Int[]
+    actuators = Int[]
+    ## actuators = [
+    ##     RB.RegisterActuator(
+    ##         1,
+    ##         collect(1:ncables_prism),
+    ##         zeros(ncables_prism)
+    ##     ),
+    ## ]
+    hub = RB.ControlHub(
+        st,
+        gauges,
+        actuators,
+        RB.Coalition(st,gauges,actuators)
+    )
+    bot = RB.Robot(st,hub)
 end
diff --git a/examples/robots/prism.jl b/examples/robots/prism.jl
index 38c7769..9d0488f 100644
--- a/examples/robots/prism.jl
+++ b/examples/robots/prism.jl
@@ -1,266 +1,276 @@
 function prisms(;
-    κ = nothing,
-    r = 1.0,
-    r1 = 1.0,
-    b = 2.0,
-    m = 4,
-    α = 2π/m,
-    h = nothing,
-    n = 1,
-    θ = 1.25α,
-    hasplate = true,
-)
-if h isa Nothing
-    @assert θ>α
-    c = sqrt(
-        r^2 + r1^2 -2*r*r1*cos(θ)
+        κ = nothing,
+        r = 1.0,
+        r1 = 1.0,
+        b = 2.0,
+        m = 4,
+        α = 2π/m,
+        h = nothing,
+        n = 1,
+        θ = 1.25α,
+        hasplate = true,
     )
-    h² = b^2-c^2
-    @assert h² > 0
-    h = sqrt(h²)
-end
-@show r,r1
-d, l = divrem(n,2)
-@show θ |> rad2deg
-bps = [
-    begin
-        if j |> iseven
-            ps = [
-                [r*cos(i*α),r*sin(i*α),j*h]
-                for i = 1:m
-            ]
-        else
-            ps = [
-                [r1*cos(i*α+θ),r1*sin(i*α+θ),j*h]
-                for i = 1:m
-            ]
-        end
-        ps .|> SVector{3} |> CircularArray
+    if h isa Nothing
+        @assert θ>α
+        c = sqrt(
+            r^2 + r1^2 -2*r*r1*cos(θ)
+        )
+        h² = b^2-c^2
+        @assert h² > 0
+        h = sqrt(h²)
     end
-    for j = 0:n
-]
-# fig = Figure()
-# ax = Axis3(fig[1,1])
-# scatter!(ax,reduce(vcat,bps))
-# scatter!(ax,reduce(vcat,midps))
-# fig	
-# @myshow length(bps), length(midps)
-bars = [
-    begin
-        if j |> iseven
-            [
-                make_3d_bar(
-                    m*(j-1)+i,
-                    bps[j][i],
-                    bps[j+1][i]; 
-                    #ci = ifelse(j==1,[1,2,3],Int[]),
-                    radius_ratio = 1/60,
-                ) for i = 1:m
-            ]
-        else
-            [
-                make_3d_bar(
-                    m*(j-1)+i,
-                    bps[j][i-1],
-                    bps[j+1][i-1];
-                    radius_ratio = 1/60,
-                ) for i = 1:m
-            ]
+    @show r,r1
+    d, l = divrem(n,2)
+    @show θ |> rad2deg
+    bps = [
+        begin
+            if j |> iseven
+                ps = [
+                    [r*cos(i*α),r*sin(i*α),j*h]
+                    for i = 1:m
+                ]
+            else
+                ps = [
+                    [r1*cos(i*α+θ),r1*sin(i*α+θ),j*h]
+                    for i = 1:m
+                ]
+            end
+            ps .|> SVector{3} |> CircularArray
         end
-    end
-    for j = 1:n
-]
+        for j = 0:n
+    ]
+    # fig = Figure()
+    # ax = Axis3(fig[1,1])
+    # scatter!(ax,reduce(vcat,bps))
+    # scatter!(ax,reduce(vcat,midps))
+    # fig	
+    # @myshow length(bps), length(midps)
+    bars = [
+        begin
+            if j |> iseven
+                [
+                    make_3d_bar(
+                        m*(j-1)+i,
+                        bps[j][i],
+                        bps[j+1][i]; 
+                        #ci = ifelse(j==1,[1,2,3],Int[]),
+                        radius_ratio = 1/60,
+                    ) for i = 1:m
+                ]
+            else
+                [
+                    make_3d_bar(
+                        m*(j-1)+i,
+                        bps[j][i-1],
+                        bps[j+1][i-1];
+                        radius_ratio = 1/60,
+                    ) for i = 1:m
+                ]
+            end
+        end
+        for j = 1:n
+    ]
 
-nbars = length.(bars) |> sum
-nb = nbars + 1
+    nbars = length.(bars) |> sum
+    nb = nbars + 1
 
-ro = SVector(0.0,0.0,0.0)
-nodes_raw = bps[1] |> Array
-loci = vcat(nodes_raw,)
-# nodes_ext = [
-# 	SVector(3r̄p[1],3r̄p[2],0.0)
-# 	for r̄p in nodes_raw
-# ]
-# loci = vcat(nodes_raw,nodes_ext)
-if hasplate
-    plate = make_3d_plate(
-        nb,
-        loci,
-        ro,
-        RotZ(0.0),
-        ro;
-        radius=r,
-        contactable = true,
-        m=3,
-        height=1e-2,
-        ci = collect(1:12),
-        cstr_idx = Int[],
-        loadmesh = false,
-        meshvisible = true,
-    )
-    rbs = vcat(
-        reduce(vcat,bars),
-        plate
-    ) |> TypeSortedCollection
-else
-    rbs = vcat(
-        reduce(vcat,bars),
-    ) |> TypeSortedCollection
-end
-numbered = RB.number(rbs)
-cm = CircularArray(collect(1:m))
-# sharing_elas = ElasticArray{Int}(undef, nb, 0)
-# for j = 1:n
-#     is = 2m*(j-1)
-#     for i = 1:m
-#         for k = 1:p
-#             row = zeros(Int,nb)
-#             row[is+cm[i]  ]   = k+3
-#             row[is+m+cm[i+1]] = k
-#             append!(sharing_elas,row)		
-#         end
-#     end
-# end	
-# for j = 2:n
-#     is = 2m*(j-2)+m
-#     for i = 1:m
-#         for k = 1:p
-#             row = zeros(Int,nb)
-#             row[is+cm[i]  ]   = k+3
-#             row[is+m+cm[i+2]] = k
-#             append!(sharing_elas,row)		
-#         end
-#     end
-# end
-# sharing = Matrix(sharing_elas')
-# display(sharing)
-# indexed = RB.index(rbs,sharing)
-indexed = RB.index(rbs,)
-    
-@myshow nb
-connecting_elas = ElasticArray{Int}(undef, nb, 0)
-for j = 0:n-1
-    is = m*j
-    
-    # # lower
-    # for i = 1:m
-    #     row = zeros(Int,nb)
-    #     row[is+cm[i  ]] =  1
-    #     row[is+cm[i+1]] = -1
-    #     append!(connecting_elas,row)
+    ro = SVector(0.0,0.0,0.0)
+    nodes_raw = bps[1] |> Array
+    loci = vcat(nodes_raw,)
+    # nodes_ext = [
+    # 	SVector(3r̄p[1],3r̄p[2],0.0)
+    # 	for r̄p in nodes_raw
+    # ]
+    # loci = vcat(nodes_raw,nodes_ext)
+    if hasplate
+        plate = make_3d_plate(
+            nb,
+            loci,
+            ro,
+            RotZ(0.0),
+            ro;
+            radius=r,
+            contactable = true,
+            m=3,
+            height=1e-2,
+            ci = collect(1:12),
+            cstr_idx = Int[],
+            loadmesh = false,
+            meshvisible = true,
+        )
+        rbs = vcat(
+            reduce(vcat,bars),
+            plate
+        ) |> TypeSortedCollection
+    else
+        rbs = vcat(
+            reduce(vcat,bars),
+        ) |> TypeSortedCollection
+    end
+    numbered = RB.number(rbs)
+    cm = CircularArray(collect(1:m))
+    # sharing_elas = ElasticArray{Int}(undef, nb, 0)
+    # for j = 1:n
+    #     is = 2m*(j-1)
+    #     for i = 1:m
+    #         for k = 1:p
+    #             row = zeros(Int,nb)
+    #             row[is+cm[i]  ]   = k+3
+    #             row[is+m+cm[i+1]] = k
+    #             append!(sharing_elas,row)		
+    #         end
+    #     end
+    # end	
+    # for j = 2:n
+    #     is = 2m*(j-2)+m
+    #     for i = 1:m
+    #         for k = 1:p
+    #             row = zeros(Int,nb)
+    #             row[is+cm[i]  ]   = k+3
+    #             row[is+m+cm[i+2]] = k
+    #             append!(sharing_elas,row)		
+    #         end
+    #     end
     # end
-
-    # cross
-    if j |> iseven
-        for i = 1:m
-            row = zeros(Int,nb)
-            row[is+cm[i  ]] =  1
-            row[is+cm[i-1]] = -2
-            append!(connecting_elas,row)
-        end
-        # additional cross
+    # sharing = Matrix(sharing_elas')
+    # display(sharing)
+    # indexed = RB.index(rbs,sharing)
+    indexed = RB.index(rbs,)
+        
+    @myshow nb
+    connecting_elas = ElasticArray{Int}(undef, nb, 0)
+    for j = 0:n-1
+        is = m*j
+        
+        # # lower
         # for i = 1:m
-        # 	row = zeros(Int,nb)
-        # 	row[is+cm[i  ]] =  1
-        # 	row[is+cm[i-2]] = -2
-        # 	append!(connecting_elas,row)
+        #     row = zeros(Int,nb)
+        #     row[is+cm[i  ]] =  1
+        #     row[is+cm[i+1]] = -1
+        #     append!(connecting_elas,row)
         # end
-    else
+
+        # cross
+        if j |> iseven
+            for i = 1:m
+                row = zeros(Int,nb)
+                row[is+cm[i  ]] =  1
+                row[is+cm[i-1]] = -2
+                append!(connecting_elas,row)
+            end
+            # additional cross
+            # for i = 1:m
+            # 	row = zeros(Int,nb)
+            # 	row[is+cm[i  ]] =  1
+            # 	row[is+cm[i-2]] = -2
+            # 	append!(connecting_elas,row)
+            # end
+        else
+            for i = 1:m
+                row = zeros(Int,nb)
+                row[is+cm[i+1]] = -2
+                row[is+cm[i  ]] =  1
+                append!(connecting_elas,row)
+            end
+            # addtional cross
+            # for i = 1:m
+            # 	row = zeros(Int,nb)
+            # 	row[is+cm[i+1]] = -2
+            # 	row[is+m+cm[i+1]] =  2
+            # 	append!(connecting_elas,row)
+            # end
+        end
+
+        # upper 
         for i = 1:m
             row = zeros(Int,nb)
+            row[is+cm[i  ]] =  2
             row[is+cm[i+1]] = -2
-            row[is+cm[i  ]] =  1
             append!(connecting_elas,row)
         end
-        # addtional cross
-        # for i = 1:m
-        # 	row = zeros(Int,nb)
-        # 	row[is+cm[i+1]] = -2
-        # 	row[is+m+cm[i+1]] =  2
-        # 	append!(connecting_elas,row)
-        # end
     end
+    ncables_prism = size(connecting_elas,2)
+    connecting_matrix = Matrix(connecting_elas')
+    ncables = size(connecting_matrix,1)
+    # @assert ncables == ncables_prism + ncables_outer
+    @show ncables
+    mat_cable = filter(
+        row->row.name == "Nylon 66",
+        RB.material_properties
+    )[1]
+    diameter = 1e-3Unitful.m
+    cable_length = 0.1Unitful.m
+    κ = (mat_cable.modulus_elas)*π*(diameter/2)^2/cable_length
+    # @show κ
+    @show uconvert(Unitful.N/Unitful.m,κ),ustrip(Unitful.N/Unitful.m,κ)
+    spring_dampers = [RB.DistanceSpringDamper3D(0.0,   ustrip(Unitful.N/Unitful.m,κ),0.0;slack=true) for i = 1:ncables_prism]
 
-    # upper 
-    for i = 1:m
-        row = zeros(Int,nb)
-        row[is+cm[i  ]] =  2
-        row[is+cm[i+1]] = -2
-        append!(connecting_elas,row)
-    end
-end
-ncables_prism = size(connecting_elas,2)
-connecting = Matrix(connecting_elas')
-# display(connecting)
-connected = RB.connect(rbs,connecting)
-
-ncables = size(connecting,1)
-# @assert ncables == ncables_prism + ncables_outer
-@show ncables
-mat_cable = filter(
-    row->row.name == "Nylon 66",
-    material_properties
-)[1]
-diameter = 1e-3Unitful.m
-cable_length = 0.1Unitful.m
-κ = (mat_cable.modulus_elas)*π*(diameter/2)^2/cable_length
-# @show κ
-@show uconvert(Unitful.N/Unitful.m,κ),ustrip(Unitful.N/Unitful.m,κ)
-cables_prism = [RB.DistanceSpringDamper3D(0.0,   ustrip(Unitful.N/Unitful.m,κ),0.0;slack=true) for i = 1:ncables_prism]
-cables = cables_prism
-acs = [
-    RB.RegisterActuator(
-        1,
-        collect(1:ncables_prism),
-        zeros(ncables_prism)
-    ),
-]
-apparatuses = (cables = cables,)
-hub = (actuators = acs,)
+    # display(connecting_matrix)
+    cables = RB.connect(rbs,spring_dampers;connecting_matrix,)
 
+    csts_bar2bar = [
+        begin 
+            hen = bars[j+1][cm[i+2]]
+            @show hen.prop.id
+            egg = bars[j][cm[i]]  
+            @show egg.prop.id
+            RB.PinJoint(
+                ncables+i+m*(j-1),
+                RB.Hen2Egg(
+                    RB.Signifier(hen,1,1),
+                    RB.Signifier(egg,2,1)
+                )
+            )
+        end
+        for j = 1:d for i = 1:m
+    ]
 
-csts_bar2bar = [
-    begin 
-        hen = bars[j+1][cm[i+2]]
-        @show hen.prop.id
-        egg = bars[j][cm[i]]  
-        @show egg.prop.id
-        RB.PinJoint(
-            i+m*(j-1),
-            RB.Hen2Egg(
-                i+m*(j-1),
-                RB.Signifier(hen,1,1),
-                RB.Signifier(egg,2,1)
+    if hasplate
+        csts_bar2plate = [
+            RB.PinJoint(
+                ncables+m*d+i,
+                RB.Hen2Egg(
+                    RB.Signifier(plate,i,1),
+                    RB.Signifier(bars[1][cm[i]],1,1),
+                )
             )
+            for i = 1:m
+        ]
+        csts = vcat(
+            csts_bar2plate,
+            csts_bar2bar,
         )
+    else
+        csts = csts_bar2bar
     end
-    for j = 1:d for i = 1:m
-]
 
-if hasplate
-    csts_bar2plate = [
-        RB.PinJoint(
-            m*d+i,
-            RB.Hen2Egg(
-                m*d+i,
-                RB.Signifier(plate,i,1),
-                RB.Signifier(bars[1][cm[i]],1,1),
-            )
-        )
-        for i = 1:m
-    ]
-    csts = vcat(
-        csts_bar2plate,
-        csts_bar2bar,
+    apparatuses = TypeSortedCollection(
+        vcat(cables,csts)
     )
-else
-    csts = csts_bar2bar
-end
 
-jointedmembers = RB.join(csts,indexed)
+    indexed = RB.index(rbs, apparatuses)
+    numbered = RB.number(rbs, apparatuses)
+    cnt = RB.Connectivity(
+        numbered, 
+        indexed,
+    )
+    st = RB.Structure(rbs,apparatuses,cnt)
 
-cnt = RB.Connectivity(numbered,indexed,@eponymtuple(connected,),jointedmembers)
+    gauges = Int[]
+    actuators = Int[]
+    ## actuators = [
+    ##     RB.RegisterActuator(
+    ##         1,
+    ##         collect(1:ncables_prism),
+    ##         zeros(ncables_prism)
+    ##     ),
+    ## ]
+    hub = RB.ControlHub(
+        st,
+        gauges,
+        actuators,
+        RB.Coalition(st,gauges,actuators)
+    )
 
-st = RB.Structure(rbs,apparatuses,cnt)
-bot = RB.Robot(st,hub)
+    bot = RB.Robot(st,hub)
 end
\ No newline at end of file
diff --git a/examples/robots/superball.jl b/examples/robots/superball.jl
index 235f509..e198da9 100644
--- a/examples/robots/superball.jl
+++ b/examples/robots/superball.jl
@@ -10,7 +10,8 @@ function superball(c=0.0;
         l = 1.7/2,
         d = l/2,
         k = 4000.0,
-        constrained = true,
+        visible = true,
+        constrained = false,
         loadmesh = true,
     )
 
@@ -40,7 +41,7 @@ function superball(c=0.0;
             m = 5.0,
             μ,
             e,
-            visible = ifelse(i==1 && constrained,true,false),
+            visible,
             ci = ifelse(i==1 && constrained,collect(1:6),Int[]),
             cstr_idx = ifelse(i==1 && constrained,Int[],[1]),
             loadmesh,
@@ -81,19 +82,19 @@ function superball(c=0.0;
             [5,6]
         ]
     )
-    matrix_cnt = zeros(Int,ncables,6)
+    connecting_matrix = zeros(Int,ncables,6)
     for lev = 1:3
-        matrix_cnt[8(lev-1)+1, [nb[lev][1],nb[lev+1][1]]] = [1, -1]
-        matrix_cnt[8(lev-1)+2, [nb[lev][1],nb[lev+1][1]]] = [2, -1]
-        matrix_cnt[8(lev-1)+3, [nb[lev][1],nb[lev+1][2]]] = [1, -2]
-        matrix_cnt[8(lev-1)+4, [nb[lev][1],nb[lev+1][2]]] = [2, -2]
-        matrix_cnt[8(lev-1)+5, [nb[lev][2],nb[lev+1][2]]] = [1, -1]
-        matrix_cnt[8(lev-1)+6, [nb[lev][2],nb[lev+1][2]]] = [2, -1]
-        matrix_cnt[8(lev-1)+7, [nb[lev][2],nb[lev+1][1]]] = [1, -2]
-        matrix_cnt[8(lev-1)+8, [nb[lev][2],nb[lev+1][1]]] = [2, -2]
+        connecting_matrix[8(lev-1)+1, [nb[lev][1],nb[lev+1][1]]] = [1, -1]
+        connecting_matrix[8(lev-1)+2, [nb[lev][1],nb[lev+1][1]]] = [2, -1]
+        connecting_matrix[8(lev-1)+3, [nb[lev][1],nb[lev+1][2]]] = [1, -2]
+        connecting_matrix[8(lev-1)+4, [nb[lev][1],nb[lev+1][2]]] = [2, -2]
+        connecting_matrix[8(lev-1)+5, [nb[lev][2],nb[lev+1][2]]] = [1, -1]
+        connecting_matrix[8(lev-1)+6, [nb[lev][2],nb[lev+1][2]]] = [2, -1]
+        connecting_matrix[8(lev-1)+7, [nb[lev][2],nb[lev+1][1]]] = [1, -2]
+        connecting_matrix[8(lev-1)+8, [nb[lev][2],nb[lev+1][1]]] = [2, -2]
     end
-    # display(matrix_cnt)
-    cables = RB.connect(rigdibodies, spring_dampers, matrix_cnt, 0)
+    # display(connecting_matrix)
+    cables = RB.connect(rigdibodies, spring_dampers; connecting_matrix, istart=0)
 
     apparatuses = TypeSortedCollection(
         cables
@@ -106,7 +107,14 @@ function superball(c=0.0;
         numbered, 
         indexed,
     )
-
     st = RB.Structure(rigdibodies, apparatuses, cnt, )
-    bot = RB.Robot(st,)
+    gauges = Int[]
+    actuators = Int[]
+    hub = RB.ControlHub(
+        st,
+        gauges,
+        actuators,
+        RB.Coalition(st,gauges,actuators)
+    )
+    bot = RB.Robot(st,hub)
 end
\ No newline at end of file
diff --git a/examples/robots/tower.jl b/examples/robots/tower.jl
index 0abe091..30a39b4 100644
--- a/examples/robots/tower.jl
+++ b/examples/robots/tower.jl
@@ -98,43 +98,49 @@ function tower(;k=nothing)
     end	
 
     display(cnt_matrix_elas)
-    cnt_matrix = Matrix(cnt_matrix_elas')
-    ncables = size(cnt_matrix,1)
+    connecting_matrix = Matrix(cnt_matrix_elas')
+    ncables = size(connecting_matrix,1)
     if k isa Nothing
-        cables = [RB.DistanceSpringDamper3D(0.0,100.0,0.0;slack=false) for i = 1:ncables]
+        spring_dampers = [RB.DistanceSpringDamper3D(0.0,100.0,0.0;slack=false) for i = 1:ncables]
     else
-        cables = [RB.DistanceSpringDamper3D(0.0,k[i],0.0;slack=false) for i = 1:ncables]
+        spring_dampers = [RB.DistanceSpringDamper3D(0.0,k[i],0.0;slack=false) for i = 1:ncables]
     end
-    apparatuses = (cables = cables,)
-    connected = RB.connect(rbs,cnt_matrix)
+    cables = RB.connect(rbs,spring_dampers;connecting_matrix)
 
     cst1 = RB.PinJoint(
-        1,
+        ncables+1,
         RB.Hen2Egg(
-            1,
             RB.Signifier(base,1),
             RB.Signifier(bar,1),
         )
     )
     
     cst2 = RB.PinJoint(
-        2,
+        ncables+2,
         RB.Hen2Egg(
-            2,
             RB.Signifier(bar,2),
             RB.Signifier(top,1)
         )
     )
 
-    jointedmembers = RB.join((cst1,cst2,),indexed)
+    apparatuses = TypeSortedCollection(
+        vcat(cables,cst1,cst2)
+    )
 
+    indexed = RB.index(rbs, apparatuses)
+    numbered = RB.number(rbs, apparatuses)
     cnt = RB.Connectivity(
-            numbered,
-            indexed,
-            @eponymtuple(connected,),
-            jointedmembers
-        )
-
+        numbered, 
+        indexed,
+    )
     st = RB.Structure(rbs,apparatuses,cnt)
-    bot = RB.Robot(st,)
+    gauges = Int[]
+    actuators = Int[]
+    hub = RB.ControlHub(
+        st,
+        gauges,
+        actuators,
+        RB.Coalition(st,gauges,actuators)
+    )
+    bot = RB.Robot(st,hub)
 end
\ No newline at end of file
diff --git a/examples/robots/two_tri.jl b/examples/robots/two_tri.jl
index 97098b9..6f4e3cb 100644
--- a/examples/robots/two_tri.jl
+++ b/examples/robots/two_tri.jl
@@ -15,23 +15,15 @@ function two_tri(;k=100.0,c=0.0,ratio=0.8)
     )
 
     rbs = TypeSortedCollection((rb1,rb2))
-    numbered = RB.number(rbs)
-    matrix_sharing = [
-        1 1;
-        2 2;
-    ]
-    indexed = RB.index(rbs,matrix_sharing)
     #
     ncables = 4
     restlen1 = 0.05
     restlens = fill(restlen1,ncables)
     ks = fill(k,ncables)
     cs = fill(c,ncables)
-    cables = [RB.DistanceSpringDamper2D(restlens[i],ks[i],cs[i];slack=true) for i = 1:ncables]
-    acs = [RB.RegisterActuator(1,1:ncables,restlens,RB.Uncoupled())]
-    apparatuses = (cables = cables,)
-    hub = (actuators = acs,)
-    cnt_matrix_cables = [
+    spring_dampers = [RB.DistanceSpringDamper2D(restlens[i],ks[i],cs[i];slack=true) for i = 1:ncables]
+
+    connecting_matrix = [
         3 -2 ;
        -2  3 ;
         5 -2 ;
@@ -39,7 +31,7 @@ function two_tri(;k=100.0,c=0.0,ratio=0.8)
         # 5 -2 ;
         # 4 -3 ;
     ]
-    connected = RB.connect(rbs,cnt_matrix_cables)
+    cables = RB.connect(rbs,spring_dampers;connecting_matrix)
     #
     # cst1 = RB.PinJoint(
     #     1,
@@ -63,13 +55,32 @@ function two_tri(;k=100.0,c=0.0,ratio=0.8)
 
     # jointedmembers = RB.join((cst1,),indexed)
 
+    apparatuses = TypeSortedCollection(
+        cables
+    )
+
+    numbered = RB.number(rbs,apparatuses)
+    sharing_matrix = [
+        1 1;
+        2 2;
+    ]
+    indexed = RB.index(rbs,apparatuses;sharing_matrix)
+
     cnt = RB.Connectivity(
         numbered,
         indexed,
-        @eponymtuple(connected,),
-        # jointedmembers
     )
 
     st = RB.Structure(rbs,apparatuses,cnt)
+    ## acs = [RB.RegisterActuator(1,1:ncables,restlens,RB.Uncoupled())]
+    ## hub = (actuators = acs,)
+    gauges = Int[]
+    actuators = Int[]
+    hub = RB.ControlHub(
+        st,
+        gauges,
+        actuators,
+        RB.Coalition(st,gauges,actuators)
+    )
     bot = RB.Robot(st,hub)
 end
\ No newline at end of file
diff --git a/src/Rible.jl b/src/Rible.jl
index d072d5c..5a7431b 100644
--- a/src/Rible.jl
+++ b/src/Rible.jl
@@ -70,6 +70,8 @@ include("utils.jl")
 
 include("members/loci.jl")
 
+include("members/forces/gravitational.jl")
+
 function get_num_of_cstr end
 function get_num_of_coords end
 function get_num_of_dof end
@@ -97,6 +99,7 @@ function get_joint_forces_jacobian! end
 function get_joint_velocity_jacobian! end
 function find_independent_free_idx end
 function nullspace_mat end
+function kinetic_energy_coords end
 
 include("members/rigids/natural_coordinates/NCF.jl")
 # using .NCF
diff --git a/src/control/control.jl b/src/control/control.jl
index 1f0bf18..75d3435 100644
--- a/src/control/control.jl
+++ b/src/control/control.jl
@@ -191,7 +191,7 @@ function cost_jacobian!(∂ϕ∂qᵀ,∂ϕ∂q̇ᵀ,∂ϕ∂pᵀ,bot::Robot,q::A
     update_bodies!(structure)
     update_apparatuses!(structure)
     if gravity
-        apply_gravity!(structure)
+        apply_field!(structure)
     end
     assemble_forces!(structure)
     M⁻¹ = assemble_M⁻¹(structure)
@@ -233,7 +233,7 @@ function generalized_force_jacobian!(∂F∂θ,structure::Structure,hub,policy::
     update_bodies!(structure)
     update_apparatuses!(structure)
     if gravity
-        apply_gravity!(structure)
+        apply_field!(structure)
     end
     control = (x) -> Lux.apply(policy.nt.actor,x,policy.nt.ps,policy.nt.st)[1]
     actuate!(bot,control,q,q̇,t)
@@ -288,6 +288,9 @@ end
 function generalized_force_jacobian!(∂F∂q̌,∂F∂q̌̇,structure::Structure,hub,policy::NoPolicy,q,q̇,t::Real,∂F∂s=nothing,s=nothing;gravity=false)
 end
 
+function generalized_force_jacobian!(∂F∂q̌,∂F∂q̌̇,structure::Structure,hub,policy::NoPolicy,q::AbstractVector,q̇::AbstractVector,t::Real;gravity=false)
+end
+
 function set_restlen!(structure,u)
     for (i,s) in enumerate(structure.apparatuses.apparatuses)
         s.state.restlen = u[i]
@@ -322,4 +325,8 @@ end
 function get_num_of_params(policy::AbstractPolicy)
     (;ps) = policy.nt
     length(ps)
+end
+
+function get_num_of_params(policy::NoPolicy)
+    0
 end
\ No newline at end of file
diff --git a/src/get.jl b/src/get.jl
index cbaf256..c96e04e 100644
--- a/src/get.jl
+++ b/src/get.jl
@@ -76,7 +76,7 @@ function get_local_coords(bodies,numbered::Numbered)
     ret
 end
 
-function get_local_coords(body::RigidBody,r̄p)
+function get_local_coords(body::AbstractRigidBody,r̄p)
     to_local_coords(body.coords.nmcs,r̄p)
 end
 
@@ -210,6 +210,8 @@ get_bodies(bot::Robot) = get_bodies(bot.structure)
 get_bodies(st::AbstractStructure) = sort(st.bodies)
 get_apparatuses(bot::Robot) = get_apparatuses(bot.structure)
 get_apparatuses(st::AbstractStructure) = sort(st.apparatuses)
+get_cables(bot::Robot) = get_cables(bot.structure)
+get_cables(st::AbstractStructure) = filter!((x)->x.joint isa CableJoint, sort(st.apparatuses))
 
 get_rigidbars(bot::Robot) = get_rigidbars(bot.structure)
 
@@ -233,7 +235,8 @@ Return System DistanceSpringDamper stiffness coefficients
 $(TYPEDSIGNATURES)
 """
 function get_cables_stiffness(st::Structure)
-    [s.k for s in st.apparatuses.cables]
+    cables = get_cables(st)
+    [s.force.k for s in cables]
 end
 
 """
@@ -241,11 +244,13 @@ Return System DistanceSpringDamper current Length.
 $(TYPEDSIGNATURES)
 """
 function get_cables_len(st::Structure)
-    [s.state.length for s in st.apparatuses.cables]
+    cables = get_cables(st)
+    [s.force.state.length for s in cables]
 end
 
 function get_cables_len_dot(st::Structure)
-    [s.state.lengthdot for s in st.apparatuses.cables]
+    cables = get_cables(st)
+    [s.force.state.lengthdot for s in cables]
 end
 
 """
@@ -253,7 +258,8 @@ Return System DistanceSpringDamper deformation。
 $(TYPEDSIGNATURES)
 """
 function get_cables_deform(st::Structure)
-    [s.state.length - s.state.restlen for s in st.apparatuses.cables]
+    cables = get_cables(st)
+    [s.force.state.length - s.force.state.restlen for s in cables]
 end
 
 """
@@ -261,7 +267,8 @@ Return System DistanceSpringDamper Restlength。
 $(TYPEDSIGNATURES)
 """
 function get_cables_restlen(st::Structure)
-    [s.state.restlen for s in st.apparatuses.cables]
+    cables = get_cables(st)
+    [s.force.state.restlen for s in cables]
 end
 
 """
@@ -269,7 +276,8 @@ Return System DistanceSpringDamper Tension.
 $(TYPEDSIGNATURES)
 """
 function get_cables_tension(st::Structure)
-    [s.state.tension for s in st.apparatuses.cables]
+    cables = get_cables(st)
+    [s.force.state.tension for s in cables]
 end
 
 """
@@ -277,8 +285,9 @@ Set cables' tension
 $(TYPEDSIGNATURES)
 """
 function set_cables_tension!(st::Structure,fs)
-    for (s,f) in zip(st.apparatuses.cables,fs)
-        s.state.tension = f
+    cables = get_cables(st)
+    for (s,f) in zip(cables,fs)
+        s.force.state.tension = f
     end
 end
 
@@ -288,7 +297,8 @@ Return System DistanceSpringDamper force density。
 $(TYPEDSIGNATURES)
 """
 function get_cables_force_density(st::Structure)
-    [s.state.tension/s.state.length for s in st.apparatuses.cables]
+    cables = get_cables(st)
+    [s.force.state.tension/s.force.state.length for s in cables]
 end
 
 """
@@ -303,13 +313,14 @@ function get_original_restlen(botinput::Robot)
 end
 
 function force_densities_to_restlen(st::Structure,γs)
+    cables = get_cables(st)
     [
     begin
-        l = s.state.length
-        l̇ = s.state.lengthdot
+        l = s.force.state.length
+        l̇ = s.force.state.lengthdot
         k = s.k
         c = s.c
         u = l-(γ*l-c*l̇)/k
     end
-        for (γ,s) in zip(γs,st.apparatuses.cables)]
+        for (γ,s) in zip(γs,cables)]
 end
diff --git a/src/mechanics/contact.jl b/src/mechanics/contact.jl
index 2be7947..9e04743 100644
--- a/src/mechanics/contact.jl
+++ b/src/mechanics/contact.jl
@@ -1,17 +1,24 @@
 abstract type AbstractField end
+struct NoField <: AbstractField end
 abstract type AbstractGravity <: AbstractField end
-struct NoGravity <: AbstractGravity end
 struct EarthSurfaceGravity{T} <: AbstractGravity 
     acceleration::T
+    function EarthSurfaceGravity(g=9.81)
+        new{typeof(g)}(g)
+    end
 end
 
-Gravity(g=9.81) = EarthSurfaceGravity(g)
-
+struct EarthGravitation{T} <: AbstractGravity 
+    std_gra::T
+end
 
-has_gravity(::NoGravity) = false
+has_gravity(::NoField) = false
 has_gravity(::EarthSurfaceGravity) = true
+has_gravity(::EarthGravitation) = true
 
 abstract type AbstractGeometry end
+struct NoGeometry <: AbstractGeometry end
+
 
 abstract type AbstractEnvironment end
 
@@ -21,7 +28,23 @@ struct StaticEnvironment{geometryType,fieldType} <: AbstractEnvironment
     geometry::geometryType
     field::fieldType
 end
-function StaticContactSurfaces(surfaces,gravity=Gravity())
+
+GravityEnv(g=9.81) = StaticEnvironment(
+    NoGeometry(),
+    EarthSurfaceGravity(g),
+)
+
+EmptyEnv() = StaticEnvironment(
+    NoGeometry(),
+    NoField(),
+)
+
+GravitationalEnv(std_gra=std_gra) = StaticEnvironment(
+    NoGeometry(),
+    EarthGravitation(std_gra),
+)
+
+function StaticContactSurfaces(surfaces,gravity=EarthSurfaceGravity(9.81))
     StaticEnvironment(
         surfaces,
         gravity
@@ -52,7 +75,7 @@ end
 
 function Contact(id,μ,e)
     active = false
-    persistent = false
+    persistent = true
     i = one(μ)
     o = zero(μ)
     gap = i
diff --git a/src/mechanics/dynamic_relax.jl b/src/mechanics/dynamic_relax.jl
index 470274e..5aefaac 100644
--- a/src/mechanics/dynamic_relax.jl
+++ b/src/mechanics/dynamic_relax.jl
@@ -27,7 +27,7 @@ function initialize_GDR(structure,F::Nothing;gravity=true)
         update_bodies!(structure,x)
         update_apparatuses!(structure)
         if gravity
-            apply_gravity!(structure)
+            apply_field!(structure)
         end
         assemble_forces!(F,structure)
         # @show abs.(F-Q) |> maximum
diff --git a/src/mechanics/dynamics.jl b/src/mechanics/dynamics.jl
index 136908e..5c3265e 100644
--- a/src/mechanics/dynamics.jl
+++ b/src/mechanics/dynamics.jl
@@ -53,7 +53,7 @@ function generalized_force!(F,bot::Robot,::NoPolicy,q,q̇,t,s=nothing;gravity=tr
     lazy_update_bodies!(structure,q,q̇)
     update_apparatuses!(structure, s)
     if gravity
-        apply_gravity!(structure;factor=1)
+        apply_field!(structure;factor=1)
     end
     ## actuate!(bot,q,q̇,t)
     assemble_forces!(F,structure)
@@ -67,27 +67,13 @@ function generalized_force!(F,bot::Robot,policy::AbstractPolicy,q,q̇,t,s=nothin
     lazy_update_bodies!(structure,q,q̇)
     update_apparatuses!(structure, s)
     if gravity
-        apply_gravity!(structure;factor=1)
+        apply_field!(structure;factor=1)
     end
     assemble_forces!(F,structure)
     user_defined_force!(F,t)
 end
 
-function generalized_force_hole!(F,bot::Robot,policy::AbstractPolicy,q,q̇,t,s,s_temp;gravity=true,(user_defined_force!)=(F,t)->nothing)
-    (;structure) = bot
-    clear_forces!(structure)
-    actuate!(bot,policy,q,q̇,t)
-    lazy_update_bodies!(structure,q,q̇)
-    # update_apparatuses!(structure)
-    update_apparatuses_hole!(structure, s, s_temp)
-    if gravity
-        apply_gravity!(structure;factor=1)
-    end
-    assemble_forces!(F,structure)
-    user_defined_force!(F,t)
-end
-
-function generalized_force_jacobian!(∂F∂q̌,∂F∂q̌̇,bot::Robot,q,q̇,t,∂F∂s,s=nothing)
+function generalized_force_jacobian!(∂F∂q̌,∂F∂q̌̇,bot::Robot,q,q̇,t,∂F∂s=nothing,s=nothing)
     generalized_force_jacobian!(∂F∂q̌,∂F∂q̌̇,bot,NoPolicy(),q,q̇,t,∂F∂s,s)
 end
 
@@ -136,23 +122,6 @@ function generalized_force_jacobian!(∂F∂s,structure::AbstractStructure,q,q̇
     end
 end
 
-function generalized_force_jacobian_hole!(∂F∂q̌,∂F∂q̌̇,∂F∂s̄,∂ζ∂q,ζ,bot::Robot,policy::AbstractPolicy,q,q̇,t,cluster_cache,s=nothing)
-    (;structure,hub) = bot
-    ∂F∂q̌ .= 0
-    ∂F∂q̌̇ .= 0
-    ∂F∂s̄ .= 0
-    ∂ζ∂q .= 0
-    ζ .= 0
-    clear_forces!(structure)
-    lazy_update_bodies!(structure,q,q̇)
-    update_apparatuses!(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)
-    build_ζ_hole!(ζ, cluster_cache, structure)
-    build_∂ζ∂q!(∂ζ∂q, cluster_cache, structure)
-end
-
 struct ContactCache{cacheType}
     cache::cacheType
 end
@@ -327,7 +296,7 @@ function activate_frictional_contacts!(structure,contact_env::StaticEnvironment,
     (;indexed,numbered) = structure.connectivity
     (;bodyid2sys_loci_idx) = numbered
     (;num_of_bodies) = indexed
-    (;geometry) = contact_env
+    surfaces = contact_env.geometry
     T = eltype(q)
     nq = length(q)
     na = 0
@@ -342,7 +311,7 @@ function activate_frictional_contacts!(structure,contact_env::StaticEnvironment,
             for pid in eachindex(loci_states)
                 locus_state = loci_states[pid]
                 (;frame,contact_state) = locus_state
-                gap, normal = contact_gap_and_normal(frame.position,geometry)
+                gap, normal = contact_gap_and_normal(frame.position,surfaces)
                 if !checkpersist
                     contact_state.active = false
                 end
@@ -489,7 +458,7 @@ function activate_contacts!(structure,contact_env::StaticEnvironment,solver_cach
     (;indexed,numbered) = structure.connectivity
     (;bodyid2sys_loci_idx) = numbered
     (;num_of_bodies) = indexed
-    (;geometry) = contact_env
+    surfaces = contact_env.geometry
     T = eltype(q)
     nq = length(q)
     na = 0
diff --git a/src/mechanics/dynamics_solvers/Moreau_family/Moreau_CCP_constant_mass.jl b/src/mechanics/dynamics_solvers/Moreau_family/Moreau_CCP_constant_mass.jl
index 60c1a3a..3c14580 100644
--- a/src/mechanics/dynamics_solvers/Moreau_family/Moreau_CCP_constant_mass.jl
+++ b/src/mechanics/dynamics_solvers/Moreau_family/Moreau_CCP_constant_mass.jl
@@ -6,6 +6,7 @@ end
 function generate_cache(
         simulator::Simulator{<:DynamicsProblem{
             RobotType,
+            policyType,
             EnvType,
             RestitutionFrictionCombined{NewtonRestitution,CoulombFriction}
         }},
@@ -15,12 +16,17 @@ function generate_cache(
         },
         ::Val{true};
         dt,kargs...
-    )   where {RobotType,EnvType}
+    )   where {RobotType,policyType,EnvType}
     (;prob) = simulator
-    (;bot,env) = prob
+    (;bot,policy,env) = prob
     (;structure) = bot
-    F!(F,q,q̇,t) = generalized_force!(F,bot,q,q̇,t;gravity=true)
-    Jac_F!(∂F∂q̌,∂F∂q̌̇,q,q̇,t) = generalized_force_jacobain!(∂F∂q̌,∂F∂q̌̇,bot,q,q̇,t)
+    options = merge(
+        (gravity=true,factor=1,checkpersist=true), #default
+        prob.options,
+        solver.options,
+    )
+    F!(F,q,q̇,t) = generalized_force!(F,bot,policy,q,q̇,t;gravity=options.gravity)
+    Jac_F!(∂F∂q̌,∂F∂q̌̇,q,q̇,t) = generalized_force_jacobian!(∂F∂q̌,∂F∂q̌̇,bot,policy,q,q̇,t)
     
     M = Matrix(assemble_M(structure))
     Φ = make_cstr_function(bot)
diff --git a/src/mechanics/dynamics_solvers/Moreau_family/Moreau_constant_mass.jl b/src/mechanics/dynamics_solvers/Moreau_family/Moreau_constant_mass.jl
index 7d58b17..7717f25 100644
--- a/src/mechanics/dynamics_solvers/Moreau_family/Moreau_constant_mass.jl
+++ b/src/mechanics/dynamics_solvers/Moreau_family/Moreau_constant_mass.jl
@@ -30,7 +30,7 @@ function generate_cache(
     A = make_cstr_jacobian(structure)
     Φ = make_cstr_function(structure)
     F!(F,q,q̇,t) = generalized_force!(F,bot,q,q̇,t)
-    Jac_F!(∂F∂q̌,∂F∂q̌̇,q,q̇,t) = generalized_force_jacobain!(∂F∂q̌,∂F∂q̌̇,bot,q,q̇,t)
+    Jac_F!(∂F∂q̌,∂F∂q̌̇,q,q̇,t) = generalized_force_jacobian!(∂F∂q̌,∂F∂q̌̇,bot,q,q̇,t)
     ∂Aᵀλ∂q(q::AbstractVector,λ) = cstr_forces_jacobian(structure,q,λ)
     ∂Aq̇∂q(q::AbstractVector,q̇) = cstr_velocity_jacobian(structure,q,q̇)
     q̌0 = traj.q̌[begin]
diff --git a/src/mechanics/dynamics_solvers/Zhong06_family/Adjoint_Zhong06_constant_mass.jl b/src/mechanics/dynamics_solvers/Zhong06_family/Adjoint_Zhong06_constant_mass.jl
index c5a4573..ab67e38 100644
--- a/src/mechanics/dynamics_solvers/Zhong06_family/Adjoint_Zhong06_constant_mass.jl
+++ b/src/mechanics/dynamics_solvers/Zhong06_family/Adjoint_Zhong06_constant_mass.jl
@@ -199,7 +199,7 @@ function generate_cache(
     ∂Fₘ∂θ = zeros(T,nq,nθ)
     Jacᵏ⁺¹ₘ = zeros(T,nx,nθ)
     function ∂f∂θ!(Jacᵏ⁺¹ₘ,∂Fₘ∂θ,q,q̇,t,h)
-        generalized_force_jacobian!(∂Fₘ∂θ,bot,policy,q,q̇,t;gravity=options.gravity)
+        generalized_force_jacobian!(∂Fₘ∂θ,structure,policy,q,q̇,t;gravity=options.gravity)
         Jacᵏ⁺¹ₘ[    1: nq,1:nθ] .= -h^2/2*∂Fₘ∂θ
         Jacᵏ⁺¹ₘ[ nq+1:2nq,1:nθ] .= -h^2/2*∂Fₘ∂θ
         Jacᵏ⁺¹ₘ[2nq+1:end,1:nθ] .= 0.0
diff --git a/src/mechanics/dynamics_solvers/Zhong06_family/Adjoint_Zhong06_nonconstant_mass.jl b/src/mechanics/dynamics_solvers/Zhong06_family/Adjoint_Zhong06_nonconstant_mass.jl
index 947ee3c..1c21431 100644
--- a/src/mechanics/dynamics_solvers/Zhong06_family/Adjoint_Zhong06_nonconstant_mass.jl
+++ b/src/mechanics/dynamics_solvers/Zhong06_family/Adjoint_Zhong06_nonconstant_mass.jl
@@ -19,7 +19,7 @@ function generate_cache(
     Φ = make_cstr_function(structure)
     A = make_cstr_jacobian(structure)
     F!(F,q,q̇,t) = generalized_force!(F,bot,q,q̇,t)
-    Jac_F!(∂F∂q̌,∂F∂q̌̇,q,q̇,t) = generalized_force_jacobain!(∂F∂q̌,∂F∂q̌̇,bot,q,q̇,t)
+    Jac_F!(∂F∂q̌,∂F∂q̌̇,q,q̇,t) = generalized_force_jacobian!(∂F∂q̌,∂F∂q̌̇,bot,q,q̇,t)
     ∂Aᵀλ∂q(q,λ) = cstr_forces_jacobian(structure,q,λ)
     adjoint_traj = StructArray(
         [
diff --git a/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_CCP_constant_mass_mono.jl b/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_CCP_constant_mass_mono.jl
index 1bbbe24..cda64b8 100644
--- a/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_CCP_constant_mass_mono.jl
+++ b/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_CCP_constant_mass_mono.jl
@@ -6,6 +6,7 @@ end
 function generate_cache(
         simulator::Simulator{<:DynamicsProblem{
             RobotType,
+            policyType,
             EnvType,
             RestitutionFrictionCombined{NewtonRestitution,CoulombFriction}
         }},
@@ -15,12 +16,18 @@ function generate_cache(
         },
         ::Val{true};
         dt,kargs...
-    )   where {RobotType,EnvType}
+    )   where {RobotType,policyType,EnvType}
     (;prob) = simulator
-    (;bot,env) = prob
+    (;bot,policy,env) = prob
     (;structure) = bot
-    F!(F,q,q̇,t) = generalized_force!(F,bot,q,q̇,t;gravity=true)
-    Jac_F!(∂F∂q̌,∂F∂q̌̇,q,q̇,t) = generalized_force_jacobain!(∂F∂q̌,∂F∂q̌̇,bot,q,q̇,t)
+    options = merge(
+        (gravity=true,factor=1,checkpersist=true), #default
+        prob.options,
+        solver.options,
+    )
+    F!(F,q,q̇,t) = generalized_force!(F,bot,policy,q,q̇,t;gravity=options.gravity)
+    Jac_F!(∂F∂q̌,∂F∂q̌̇,q,q̇,t) = generalized_force_jacobian!(∂F∂q̌,∂F∂q̌̇,bot,policy,q,q̇,t)
+    ## ∂F∂θ!(∂Fₘ∂θ,q,q̇,t) = generalized_force_jacobian!(∂Fₘ∂θ,structure,policy,q,q̇,t;)
     
     M = Matrix(assemble_M(structure))
     Φ = make_cstr_function(bot)
@@ -45,13 +52,14 @@ function generate_cache(
     ) = prepare_contacts(bot,env)
     
     cache = @eponymtuple(
-        F!,Jac_F!,
+        F!,Jac_F!,#∂F∂θ!,
         M,Φ,A,Ψ,B,∂Ψ∂q,∂Aᵀλ∂q,∂Bᵀμ∂q,
         contacts_bits,
         persistent_bits,
         μs_sys,
         es_sys,
-        gaps_sys
+        gaps_sys,
+        options
     )
     Zhong06_CCP_Constant_Mass_Mono_Cache(cache)
 end
@@ -59,19 +67,19 @@ end
 function make_step_k(
         solver_cache::Zhong06_CCP_Constant_Mass_Mono_Cache,
         nq,nλ,na,
-        qₖ₋₁,vₖ₋₁,pₖ₋₁,tₖ₋₁,
+        step_data,tₖ₋₁,
         pₖ,vₖ,
         invM,
         h,mass_norm)
     (;F!,Jac_F!,M,Φ,A,∂Aᵀλ∂q) = solver_cache.cache
-
+    (;qₖ₋₁,vₖ₋₁,pₖ₋₁) = step_data
     n1 = nq
     n2 = nq+nλ
     nΛ = 3na
     nx = n2
     function ns_stepk!(
-            𝐫𝐞𝐬,𝐉,
-            Fₘ,∂F∂q,∂F∂q̇,
+            𝐫𝐞𝐬,𝐉,𝐉_data,
+            Fₘ,∂F∂q,∂F∂q̇,#∂Fₘ∂θ,
             𝐰,x,Λₖ,y,∂y∂x,
             Λ_split,y_split,
             structure,
@@ -91,6 +99,7 @@ function make_step_k(
         Aₖ₋₁ = A(qₖ₋₁)
         Aₖ   = A(qₖ)
 
+        # pₖ₋₁ = M*vₖ₋₁
         𝐫𝐞𝐬[   1:n1] .= -h.*pₖ₋₁ .+ M*(qₖ.-qₖ₋₁) .-
                         mass_norm.*transpose(Aₖ₋₁)*λₘ .-
                         (h^2)/2 .*Fₘ
@@ -101,6 +110,18 @@ function make_step_k(
         𝐉[   1:n1,n1+1:n2] .= -mass_norm.*transpose(Aₖ₋₁)
 
         𝐉[n1+1:n2,   1:n1] .=  -mass_norm.*Aₖ
+        𝐉_data .= 0.0
+        #position
+        𝐉_data[   1:nq,   1:nq] .= -M .-h^2/2 .*(1/2 .*∂F∂q .- 1/h.*∂F∂q̇) .- mass_norm.*∂Aᵀλ∂q(qₖ₋₁,λₘ)
+        #velocity
+        𝐉_data[   1:nq,nq+1:2nq] .= -h*M
+        #control
+        ## ∂F∂θ!(∂Fₘ∂θ,qₘ,q̇ₘ,tₘ)
+        ## 𝐉_data[   1:nq,2nq+1:nθ] .= -h^2/2*∂Fₘ∂θ
+        #structural
+        ## 𝐉_data[   1:n1,n1+1:n2]
+        #contact
+        ## 𝐉_data[   1:n1,n1+1:n2]
 
         if na != 0
             get_distribution_law!(structure,contact_cache,qₖ)
@@ -130,7 +151,7 @@ function make_step_k(
                 ∂y∂x[is+1:is+3,   1:n1] .= ∂v́⁺∂qₖ[is+1:is+3,:]
                 ∂y∂x[is+1:is+3,n1+1:n2] .= Dⁱₖ*∂vₖ∂λₘ
             end
-            ∂y∂x .= (I+Lv)*∂y∂x
+            ## ∂y∂x .= (I+Lv)*∂y∂x
             for i = 1:na
                 is = 3(i-1)
                 vⁱₖ₋₁ = @view v́ₖ₋₁[is+1:is+3]
@@ -143,15 +164,15 @@ function make_step_k(
                 𝐰[is+1:is+3] .= [v́ₜⁱ,0,0]
                 ∂y∂x[is+1     ,   1:n1] .+= 1/(norm(v́⁺[is+2:is+3])+1e-14)*(v́⁺[is+2]*∂v́⁺∂qₖ[is+2,:] .+ v́⁺[is+3]*∂v́⁺∂qₖ[is+3,:])
             end
-            𝐫𝐞𝐬[(n2   +1):(n2+ nΛ)] .= (h.*((I+Lv)*v́⁺ .+ 𝐰) .- h.*y)
+            𝐫𝐞𝐬[(n2   +1):(n2+ nΛ)] .= (h.*(v́⁺ .+ 𝐰) .- h.*y)
             𝐫𝐞𝐬[n2+nΛ+1:n2+2nΛ]     .= reduce(vcat,Λ_split⊙y_split)
             𝐉[      1:n1    , n2+   1:n2+ nΛ] .=  -mass_norm*h .*transpose(D)*H*(I+L)
             𝐉[n2+1:n2+ nΛ,      1:n2    ]     .=  h.*∂y∂x
             𝐉[n2+1:n2+ nΛ,    n2+nΛ+1:n2+2nΛ] .= -h.*I(nΛ)
             𝐉[n2+nΛ+1:n2+2nΛ, n2+   1:n2+ nΛ] .=  BlockDiagonal(mat.(y_split))
             𝐉[n2+nΛ+1:n2+2nΛ, n2+nΛ+1:n2+2nΛ] .=  BlockDiagonal(mat.(Λ_split))
-            if timestep == 850
-                @show 𝐉[n2+nΛ+1:n2+2nΛ,:]
+            if false
+                ## @show 𝐉[n2+nΛ+1:n2+2nΛ,:]
             end
         end
         # debug
@@ -164,14 +185,14 @@ function make_step_k(
 end
 
 function solve!(sim::Simulator,solver_cache::Zhong06_CCP_Constant_Mass_Mono_Cache;
-                dt,
-                ftol=1e-14,xtol=ftol,
-                verbose=false,verbose_contact=false,
-                maxiters=50,
-                max_restart=3,
-                progress=true,exception=true)
+        dt,
+        ftol=1e-14,xtol=ftol,
+        verbose=false,verbose_contact=false,
+        maxiters=50,max_restart=3,
+        progress=true,exception=true
+    )
     (;prob,controller,tspan,restart,totalstep) = sim
-    (;bot,env) = prob
+    (;bot,policy,env) = prob
     (;structure,traj,contacts_traj) = bot
     (;M,A,contacts_bits) = solver_cache.cache
     q0 = traj.q[begin]
@@ -187,11 +208,9 @@ function solve!(sim::Simulator,solver_cache::Zhong06_CCP_Constant_Mass_Mono_Cach
     F = zeros(T,nq)
     ∂F∂q = zeros(T,nq,nq)
     ∂F∂q̇ = zeros(T,nq,nq)
+    nθ = get_num_of_params(policy)
+    ∂Fₘ∂θ = zeros(T,nq,nθ)
     nx = nq + nλ
-    Δx = zeros(T,nx)
-    x = zero(Δx)
-    Res = zero(Δx)
-    Jac = zeros(T,nx,nx)
     mr = norm(M,Inf)
     mass_norm = mr
 
@@ -231,6 +250,7 @@ function solve!(sim::Simulator,solver_cache::Zhong06_CCP_Constant_Mass_Mono_Cach
         x = zero(Δx)
         Res = zero(Δx)
         Jac = zeros(T,nx,nx)
+        Jac_data = zeros(T,nx,nq+nq+nθ)
         Λₖ = @view x[(n2+1):n2+nΛ]
         y  = @view x[n2+nΛ+1:n2+2nΛ]
         𝐰 = zeros(T,nΛ)
@@ -252,13 +272,17 @@ function solve!(sim::Simulator,solver_cache::Zhong06_CCP_Constant_Mass_Mono_Cach
         ΔΛc_split = split_by_lengths(ΔΛc,3)
         Δyc_split = split_by_lengths(Δyc,3)
         get_frictional_directions_and_positions!(structure, contact_cache, qₖ₋₁, q̇ₖ₋₁, Λₖ)
+        step_data = ComponentArray(
+            @eponymtuple(
+                qₖ₋₁,vₖ₋₁=q̇ₖ₋₁,pₖ₋₁,
+            )
+        )
         ns_stepk! = make_step_k(
             solver_cache,
             nq,nλ,na,
-            qₖ₋₁,q̇ₖ₋₁,pₖ₋₁,tₖ₋₁,
+            step_data,tₖ₋₁,
             pₖ,q̇ₖ,
-            invM,
-            dt,mass_norm
+            invM,dt,mass_norm
         )
         restart_count = 0
         Λ_guess = 0.1
@@ -269,8 +293,8 @@ function solve!(sim::Simulator,solver_cache::Zhong06_CCP_Constant_Mass_Mono_Cach
             x[   nq+1:nq+nλ]       .= 0.0
             for iteration = 1:maxiters
                 ns_stepk!(
-                    Res,Jac,
-                    F,∂F∂q,∂F∂q̇,
+                    Res,Jac,Jac_data,
+                    F,∂F∂q,∂F∂q̇,#∂Fₘ∂θ,
                     𝐰,x,Λₖ,y,∂y∂x,
                     Λ_split,y_split,
                     structure,
@@ -290,6 +314,9 @@ function solve!(sim::Simulator,solver_cache::Zhong06_CCP_Constant_Mass_Mono_Cach
                 else # na!=0
                     μ = transpose(y)*Λₖ/nΛ
                     Δxp .= lu𝐉\(-Res)
+                    ## @show Res[n2+1:n2+2nΛ]
+                    ## @show (Jac*Δxp)[n2+1:n2+2nΛ]
+                    ## @show Δxp[n2+1:n2+2nΛ]
                     αp_Λ = find_cone_step_length(Λ_split,ΔΛp_split,J)
                     αp_y = find_cone_step_length(y_split,Δyp_split,J)
                     αpmax = min(αp_Λ,αp_y)
@@ -302,7 +329,15 @@ function solve!(sim::Simulator,solver_cache::Zhong06_CCP_Constant_Mass_Mono_Cach
                         break
                     end
                     τ = σ*μp
-                    𝐫𝐞𝐬_c_split = -τ.*𝐞_split.+((Δyp_split)⊙(ΔΛp_split))
+                    𝐫𝐞𝐬_c_split = -τ.*𝐞_split#.+((Δyp_split)⊙(ΔΛp_split))
+                    if false
+                        ## @show Λₖ, normRes, μ, cond(Jac)
+                        ## @show  qr(Jac).R |> diag
+                        @show Res[1:n1] |> norm
+                        @show Res[n1+1:n2] |> norm
+                        @show Res[n2+1:n2+nΛ] |> norm
+                        @show Res[n2+nΛ+1:n2+2nΛ] |> norm
+                    end
                     Res[n2+nΛ+1:n2+2nΛ] .+= reduce(vcat,𝐫𝐞𝐬_c_split)
                     normRes = norm(Res)
                     if  normRes < ftol
@@ -326,18 +361,25 @@ function solve!(sim::Simulator,solver_cache::Zhong06_CCP_Constant_Mass_Mono_Cach
                     # α_record[iteration] = α
                     x .+= α.*Δxc
                     μ = transpose(y)*Λₖ/nΛ
-                    if timestep == 850
-                        @show Λₖ, normRes, μ, cond(Jac)
-                        @show  qr(Jac).R |> diag
+                    if false
+                        ## @show Λₖ, normRes, μ, cond(Jac)
+                        ## @show  qr(Jac).R |> diag
+                        @show Res[n2+nΛ+1:n2+2nΛ] |> norm
+                        @show (y_split)⊙(Λ_split)
+                        @show (Δyp_split)⊙(ΔΛp_split)
                     end
+                    @show lu𝐉\(-Jac_data)
+                    ## @show Λₖ, μ
                 end
             end
             if isconverged
                 break
             end
             restart_count += 1
-            Λ_guess /= 10
-            # @warn "restarting step: $timestep, count: $restart_count, Λ_guess = $Λ_guess"
+            if na > 0
+                Λ_guess =  max(Λ_guess/10,maximum(abs.(Λₖ[begin:3:end])))
+            end
+            ## @warn "restarting step: $timestep, count: $restart_count, Λ_guess = $Λ_guess"
         end
         qₖ .= x[      1:nq]
         λₘ .= x[   nq+1:nq+nλ]
diff --git a/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_CCP_nonconstant_mass.jl b/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_CCP_nonconstant_mass.jl
index 116823a..0a3686d 100644
--- a/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_CCP_nonconstant_mass.jl
+++ b/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_CCP_nonconstant_mass.jl
@@ -19,7 +19,7 @@ function generate_cache(
     (;bot,env) = simulator.prob
     (;structure) = bot
     F!(F,q,q̇,t) = generalized_force!(F,bot,q,q̇,t)
-    Jac_F!(∂F∂q̌,∂F∂q̌̇,q,q̇,t) = generalized_force_jacobain!(∂F∂q̌,∂F∂q̌̇,bot,q,q̇,t)
+    Jac_F!(∂F∂q̌,∂F∂q̌̇,q,q̇,t) = generalized_force_jacobian!(∂F∂q̌,∂F∂q̌̇,bot,q,q̇,t)
     Mₘ = assemble_M(structure) 
     M⁻¹ₖ = assemble_M⁻¹(structure)
     ∂Mₘhq̇ₘ∂qₘ = assemble_∂Mq̇∂q(structure)
diff --git a/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_constant_mass.jl b/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_constant_mass.jl
index 51bf1e8..922e5b0 100644
--- a/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_constant_mass.jl
+++ b/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_constant_mass.jl
@@ -162,17 +162,19 @@ function solve!(sim::Simulator,cache::Zhong06_Constant_Mass_Cache;
             ##     FiniteDiff.finite_difference_jacobian!(Jac_ref,Res_stepk!,xₖ)
             ##     Jac_my = zeros(nx,nx)
             ##     Jac_stepk!(Jac_my,xₖ)
-            ##     diff_Jac = Jac_my .- Jac_ref
-                ## diff_Jac[abs.(diff_Jac).<1e-5] .= 0.0
-                ## diff_rowidx = findall((x)-> x>1e-5,norm.(eachrow(diff_Jac)))
-                ## @show diff_rowidx .- length(q̌ₖ)                
-                ## @show diff_Jac[end-4:end-3,:]
-                ## @show findall((x)->abs(x)>0,diff_Jac[114+length(q̌ₖ),:])
-                ## @show findall((x)->abs(x)>0,diff_Jac[115+length(q̌ₖ),:])
-            ##     @show Jac_my[ 1:nq̌,   nq̌+nλ+1:nx]
-            ##     @show Jac_ref[ 1:nq̌,   nq̌+nλ+1:nx]
+            ##     diff_Jac = Jac_my .- Jac_ref .|> abs
+            ## diff_Jac[(diff_Jac).<1e-8] .= 0.0
+            ## diff_rowidx = findall((x)-> x>1e-5,norm.(eachrow(diff_Jac)))
+            ## @show diff_rowidx .- length(q̌ₖ)                
+            ## @show diff_Jac[end-4:end-3,:]
+            ## @show findall((x)->abs(x)>0,diff_Jac[114+length(q̌ₖ),:])
+            ## @show findall((x)->abs(x)>0,diff_Jac[115+length(q̌ₖ),:])
+            ## @show Jac_my
+            ## @show Jac_ref
+            @show diff_Jac |> maximum
             ## end
-            ## @show maximum(abs.(diff_Jac))for iteration = 1:maxiters
+            ## @show maximum(abs.(diff_Jac))
+            ## for iteration = 1:maxiters
             ## @show iteration,D,ηs,restitution_coefficients,gaps
             for iteration = 1:maxiters
                 Res_stepk!(Res,xₖ)
@@ -201,7 +203,7 @@ function solve!(sim::Simulator,cache::Zhong06_Constant_Mass_Cache;
         q̌ₖ .= xₖ[   1:nq̌   ]
         λₖ .= xₖ[nq̌+1:nq̌+nλ]
         sₖ .= xₖ[nq̌+nλ+1:nx]
-        @show timestep, sₖ, sₖ₋₁
+        ## @show timestep, sₖ, sₖ₋₁
         Momentum_k!(p̌ₖ,p̌ₖ₋₁,qₖ,qₖ₋₁,λₖ,Ḿ,A,Aᵀₖ₋₁,mass_norm,h)
         q̌̇ₖ .= M̌⁻¹*(p̌ₖ.-M̄*q̃̇ₖ )
         #---------Step k finisher-----------
diff --git a/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_frictionless_nonconstant_mass.jl b/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_frictionless_nonconstant_mass.jl
index 1a95783..0c5970f 100644
--- a/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_frictionless_nonconstant_mass.jl
+++ b/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_frictionless_nonconstant_mass.jl
@@ -19,7 +19,7 @@ function generate_cache(
     (;bot,env) = simulator.prob
     (;structure) = bot
     F!(F,q,q̇,t) = generalized_force!(F,bot,q,q̇,t)
-    Jac_F!(∂F∂q̌,∂F∂q̌̇,q,q̇,t) = generalized_force_jacobain!(∂F∂q̌,∂F∂q̌̇,bot,q,q̇,t)
+    Jac_F!(∂F∂q̌,∂F∂q̌̇,q,q̇,t) = generalized_force_jacobian!(∂F∂q̌,∂F∂q̌̇,bot,q,q̇,t)
     Mₘ = assemble_M(structure) 
     M⁻¹ₖ = assemble_M⁻¹(structure)
     ∂Mₘq̇ₘ∂qₘ = assemble_∂Mq̇∂q(structure)
diff --git a/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_frictionless_nonconstant_mass_mono.jl b/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_frictionless_nonconstant_mass_mono.jl
index bac4e71..27660e7 100644
--- a/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_frictionless_nonconstant_mass_mono.jl
+++ b/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_frictionless_nonconstant_mass_mono.jl
@@ -19,7 +19,7 @@ function generate_cache(
     (;bot,env) = simulator.prob
     (;structure) = bot
     F!(F,q,q̇,t) = generalized_force!(F,bot,q,q̇,t)
-    Jac_F!(∂F∂q̌,∂F∂q̌̇,q,q̇,t) = generalized_force_jacobain!(∂F∂q̌,∂F∂q̌̇,bot,q,q̇,t)
+    Jac_F!(∂F∂q̌,∂F∂q̌̇,q,q̇,t) = generalized_force_jacobian!(∂F∂q̌,∂F∂q̌̇,bot,q,q̇,t)
     Mₘ = assemble_M(structure)
     ∂Mₘhq̇ₘ∂qₘ = assemble_∂Mq̇∂q(structure)
     M⁻¹ₖ = assemble_M⁻¹(structure)
diff --git a/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_nonconstant_mass.jl b/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_nonconstant_mass.jl
index a2ea54b..a19673c 100644
--- a/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_nonconstant_mass.jl
+++ b/src/mechanics/dynamics_solvers/Zhong06_family/Zhong06_nonconstant_mass.jl
@@ -19,7 +19,7 @@ function generate_cache(
     Φ = make_cstr_function(structure)
     A = make_cstr_jacobian(structure)
     F!(F,q,q̇,t) = generalized_force!(F,bot,q,q̇,t)
-    Jac_F!(∂F∂q̌,∂F∂q̌̇,q,q̇,t) = generalized_force_jacobain!(∂F∂q̌,∂F∂q̌̇,bot,q,q̇,t)
+    Jac_F!(∂F∂q̌,∂F∂q̌̇,q,q̇,t) = generalized_force_jacobian!(∂F∂q̌,∂F∂q̌̇,bot,q,q̇,t)
     cache = @eponymtuple(
         F!,Jac_F!,
         Mₘ,M⁻¹ₖ,
diff --git a/src/mechanics/inverse_statics.jl b/src/mechanics/inverse_statics.jl
index 9350cba..45d94e1 100644
--- a/src/mechanics/inverse_statics.jl
+++ b/src/mechanics/inverse_statics.jl
@@ -25,7 +25,7 @@ function build_Q̃(structure)
     foreach(structure.apparatuses) do appar
         j = appar.id
         if appar.joint isa CableJoint
-            (;hen,egg) = appar.joint
+            (;hen,egg) = appar.joint.hen2egg
             C1 = hen.body.cache.Cps[hen.pid]
             C2 = egg.body.cache.Cps[egg.pid]
             uci1 = hen.body.coords.free_idx
@@ -62,16 +62,14 @@ end
 
 function build_L̂(st)
     (;connectivity,num_of_dim) = st
-    (;connected) = connectivity.tensioned
-    (;cables) = st.apparatuses
-    ncables = length(cables)
+    cables = get_cables(st)
+    num_of_cables = length(cables)
     T = get_numbertype(st)
-    L̂ = spzeros(T, ncables*num_of_dim, ncables)
-    foreach(connected) do scnt
-        j = scnt.id
-        scable = cables[j]
+    L̂ = spzeros(T, num_of_cables*num_of_dim, num_of_cables)
+    foreach(cables) do cable
+        j = cable.id
         js = (j-1)*num_of_dim
-        L̂[js+1:js+num_of_dim,j] = scable.state.direction
+        L̂[js+1:js+num_of_dim,j] = cable.force.state.direction
     end
     L̂
 end
@@ -121,7 +119,7 @@ end
 function build_Ǧ(tginput;factor=1.0)
     st = deepcopy(tginput)
     clear_forces!(st)
-    apply_gravity!(st;factor)
+    apply_field!(st;factor)
     assemble_forces!(Ǧ,st)
 end
 
@@ -452,7 +450,7 @@ function check_static_equilibrium(tg_input,q,λ,F=nothing;gravity=false)
     update_cables_apply_forces!(st)
     check_restlen(st,get_cables_restlen(st))
     if gravity
-        apply_gravity!(st)
+        apply_field!(st)
     end
     generalized_forces = assemble_forces(st)
     if !isnothing(F)
@@ -483,17 +481,16 @@ function check_static_equilibrium_output_multipliers!(st,q,F=nothing;
         # stpt = nothing
     )
     clear_forces!(st)
-    update_bodies!(st)
+    update_bodies!(st,q)
     update_apparatuses!(st)
     # check_restlen(st,get_cables_restlen(st))
     if gravity
-        apply_gravity!(st)
+        apply_field!(st)
     end
-    assemble_forces!(generalized_forces,st)
+    generalized_forces = assemble_forces!(st)
     if !isnothing(F)
         generalized_forces .+= F[:]
     end
-    q = get_coords(st)
     c = get_local_coords(st)
     q̌ = get_free_coords(st)
     A = make_cstr_jacobian(st,q)(q̌,c)
diff --git a/src/mechanics/stiffness.jl b/src/mechanics/stiffness.jl
index 2821a37..6d102d1 100644
--- a/src/mechanics/stiffness.jl
+++ b/src/mechanics/stiffness.jl
@@ -219,7 +219,7 @@ end
 #         # μ = l .- (f./k)
 #         λ = inv(Ǎ*transpose(Ǎ))*Ǎ*Bᵀ*f
 #         # @show f,λ
-#         Ǩa = - RB.cstr_forces_jacobian(bot.structure,λ)
+#         Ǩa = RB.cstr_forces_jacobian(bot.structure,λ)
 #         𝒦a = transpose(Ň)*Ǩa*Ň |> sparse
 #         D_𝒦a = ldl(𝒦a).D.diag |> sort
 
diff --git a/src/members/abstract_body.jl b/src/members/abstract_body.jl
index 2db2eab..1a524b1 100644
--- a/src/members/abstract_body.jl
+++ b/src/members/abstract_body.jl
@@ -17,6 +17,8 @@ update_state!(body::AbstractBody,q,q̇) = update_state!(body.state,body.coords,b
 update_loci_states!(body::AbstractBody,q,q̇) = update_loci_states!(body.state,body.coords,body.cache,body.prop,q,q̇)
 stretch_loci!(body::AbstractBody,c) = stretch_loci!(body.coords,body.cache,body.prop,c)
 update_transformations!(body::AbstractBody,q) = update_transformations!(body.coords,body.cache,body.state,body.prop,q)
+kinetic_energy_coords(body::AbstractBody,q,q̇) = kinetic_energy(body.coords,body.cache,body.state,body.prop,q,q̇)
+
 function body_state2coords_state(body::AbstractBody)
     body_state2coords_state(body.state,body.coords)
 end
@@ -98,7 +100,6 @@ function cstr_velocity_jacobian(coords::NonminimalCoordinates,q̇)
     cstr_velocity_jacobian(coords.nmcs,coords.free_idx,coords.cstr_idx,q̇)
 end
 
-
 struct InertiaCache{MType,JType,GType}
     M::MType
     M⁻¹::MType
diff --git a/src/members/forces/gravitational.jl b/src/members/forces/gravitational.jl
new file mode 100644
index 0000000..9bcef7f
--- /dev/null
+++ b/src/members/forces/gravitational.jl
@@ -0,0 +1,2 @@
+const std_gra = 3.986e14#(m^3/s^-2)
+
diff --git a/src/members/loci.jl b/src/members/loci.jl
index b75c609..4452113 100644
--- a/src/members/loci.jl
+++ b/src/members/loci.jl
@@ -108,6 +108,10 @@ end
 
 to_3D(frame::CartesianFrame{3}) = frame
 
+function CartesianFrame{3}(frame::CartesianFrame{2})
+    to_3D(frame)
+end
+
 function to_3D(frame::CartesianFrame{2,M,T}) where {M,T}
     (;position,velocity,axes,angular_velocity) = frame
     o = zero(T)
diff --git a/src/members/rigids/natural_coordinates/NCF.jl b/src/members/rigids/natural_coordinates/NCF.jl
index 4eea8df..7d86ddf 100644
--- a/src/members/rigids/natural_coordinates/NCF.jl
+++ b/src/members/rigids/natural_coordinates/NCF.jl
@@ -10,7 +10,7 @@ using EponymTuples
 using ForwardDiff
 using DocStringExtensions
 
-import ..Rible: HouseholderOrthogonalization, skew, Axes, GECP, CartesianFrame
+import ..Rible: HouseholderOrthogonalization, skew, Axes, GECP, CartesianFrame, rotation_matrix
 
 import ..Rible: get_num_of_cstr, get_num_of_coords
 import ..Rible: get_num_of_dof, get_num_of_local_dims
@@ -27,8 +27,10 @@ import ..Rible: build_joint_cache, get_joint_violations!
 import ..Rible: get_joint_jacobian!
 import ..Rible: get_joint_forces_jacobian!
 import ..Rible: get_joint_velocity_jacobian!
+import ..Rible: kinetic_energy_coords
 import ..Rible: find_independent_free_idx
 import ..Rible: nullspace_mat
+import ..Rible: std_gra
 
 include("./LNC.jl")
 
diff --git a/src/members/rigids/natural_coordinates/functions.jl b/src/members/rigids/natural_coordinates/functions.jl
index e91500c..cd8bd19 100644
--- a/src/members/rigids/natural_coordinates/functions.jl
+++ b/src/members/rigids/natural_coordinates/functions.jl
@@ -94,7 +94,7 @@ function to_local_coords(nmcs::NC,r̄)
     invX̄*(r̄-r̄i)
 end
 
-function to_position_jacobian(nmcs::NC,q,c)
+function to_position_jacobian(nmcs::NC,c)
     num_of_dim = get_num_of_dims(nmcs)
     nlds = get_num_of_local_dims(nmcs)
     ncoords = get_num_of_coords(nmcs)
@@ -103,6 +103,10 @@ function to_position_jacobian(nmcs::NC,q,c)
     SMatrix{num_of_dim,ncoords}(kron(C_raw,IMatrix(num_of_dim))*conversion)
 end
 
+function to_position_jacobian(nmcs::NC,q,c)
+    to_position_jacobian(nmcs,c)
+end
+
 function to_velocity_jacobian(nmcs::NC,q,q̇,c)
     num_of_dim = get_num_of_dims(nmcs)
     nlds = get_num_of_local_dims(nmcs)
diff --git a/src/members/rigids/natural_coordinates/mass_matrix.jl b/src/members/rigids/natural_coordinates/mass_matrix.jl
index 92a8af3..c05285d 100644
--- a/src/members/rigids/natural_coordinates/mass_matrix.jl
+++ b/src/members/rigids/natural_coordinates/mass_matrix.jl
@@ -1,25 +1,61 @@
 
 # Mass matrices
-Ī2J̄(::Union{NC2D2C,NC3D3C,NC2D6C,NC3D6C},Ī)  = Ī
+Ī2J̄(::Union{NC2D2C,NC3D3C,NC2D4C,NC2D6C,NC3D6C},Ī)  = Ī
 Ī2J̄(::NC3D12C,Ī) = 1/2*tr(Ī)*I-Ī
 
-function Īg2az(nmcs,m,Īg,mass_center)
+function Īg2z(nmcs,m,Īₘ,r̄ₘ)
     (;r̄i,invX̄) = nmcs.data
-    a = invX̄*(mass_center-r̄i)
-    J̄g = Ī2J̄(nmcs,Īg)
-    J̄o = J̄g + m*mass_center*transpose(mass_center)
-    J̄i = J̄o - m*r̄i*transpose(mass_center) - m*mass_center*transpose(r̄i) + m*r̄i*transpose(r̄i)
+    cₘ = to_local_coords(nmcs,r̄ₘ)
+    J̄g = Ī2J̄(nmcs,Īₘ)
+    J̄o = J̄g + m*r̄ₘ*transpose(r̄ₘ)
+    J̄i = J̄o - m*r̄i*transpose(r̄ₘ) - m*r̄ₘ*transpose(r̄i) + m*r̄i*transpose(r̄i)
     z = invX̄*J̄i*transpose(invX̄)
     #@assert issymmetric(z)
-    a,Symmetric(z)
+    Symmetric(z)
 end
 
+const μ = std_gra
 ## Mass matrices: standard
-function make_M(nmcs::NC,m::T,Īg,mass_center) where {T} # ami (area moment of inertia tensor)
+function make_U(nmcs::NC,m::T,Īₘ,r̄ₘ,q) where {T} # ami (area moment of inertia tensor)
     num_of_dim = get_num_of_dims(nmcs)
     nld = get_num_of_local_dims(nmcs)
     ncoords = get_num_of_coords(nmcs)
-    a,z = Īg2az(nmcs,m,Īg,mass_center)
+    cₘ = to_local_coords(nmcs,r̄ₘ)
+    z = Īg2z(nmcs,m,Īₘ,r̄ₘ)
+    M̄ = zeros(T,1+nld,1+nld)
+    M̄[1,1] = m
+    M̄[2:1+nld,1] = m*cₘ
+    M̄[1,2:1+nld] = M̄[2:1+nld,1]
+    M̄[2:1+nld,2:1+nld] .= z
+    ## M_std = kron(M̄,IMatrix(num_of_dim))
+    ## cv = nmcs.conversion_to_std
+    ## M = SMatrix{ncoords,ncoords}(transpose(cv)*M_std*cv)
+    cₘ = to_local_coords(nmcs,r̄ₘ)
+    ## to_position
+    Cₘ = to_position_jacobian(nmcs,cₘ)
+    𝐜 = Cₘ*q
+    ## B̄ = M̄[[1],:] #\int S̄ 
+    ## B = kron(B̄,I(3))
+    ## @show size(B), size(q)
+    ## Bq = B*q
+    𝐜ᵀ𝐜 = 𝐜'*𝐜
+
+    D = 3𝐜*𝐜' - 𝐜ᵀ𝐜*I
+    A = kron(M̄,D)
+    ## U_gra0 = -3m*μ/𝐜ᵀ𝐜^(1/2)
+    ## U_gra1 =  3m*μ/𝐜ᵀ𝐜^(3/2)*𝐜'*Bq
+    ## @show U_gra0, U_gra1, U_gra0+U_gra1
+    U_gra2 =-1/2*μ/𝐜ᵀ𝐜^(5/2)*q'*A*q
+    ## @show U_gra2
+    U_gra = U_gra2
+end
+
+function make_M(nmcs::NC,m::T,Īₘ,r̄ₘ) where {T} # ami (area moment of inertia tensor)
+    num_of_dim = get_num_of_dims(nmcs)
+    nld = get_num_of_local_dims(nmcs)
+    ncoords = get_num_of_coords(nmcs)
+    a = to_local_coords(nmcs,r̄ₘ)
+    z = Īg2z(nmcs,m,Īₘ,r̄ₘ)
     M_raw = zeros(T,1+nld,1+nld)
     M_raw[1,1] = m
     M_raw[2:1+nld,1] = m*a
@@ -29,3 +65,227 @@ function make_M(nmcs::NC,m::T,Īg,mass_center) where {T} # ami (area moment of
     cv = nmcs.conversion_to_std
     M = SMatrix{ncoords,ncoords}(transpose(cv)*M_std*cv)
 end
+
+function gravitational_force(nmcs::NC,m::T,Īₘ,r̄ₘ,q) where {T}
+    nld = get_num_of_local_dims(nmcs)
+    a = to_local_coords(nmcs,r̄ₘ)
+    z = Īg2z(nmcs,m,Īₘ,r̄ₘ)
+    M̄ = zeros(T,1+nld,1+nld)
+    M̄[1,1] = m
+    M̄[2:1+nld,1] = m*a
+    M̄[1,2:1+nld] = M̄[2:1+nld,1]
+    M̄[2:1+nld,2:1+nld] .= z
+    
+    cₘ = to_local_coords(nmcs,r̄ₘ)
+    ## to_position
+    Cₘ = to_position_jacobian(nmcs,cₘ)
+    𝐜 = Cₘ*q
+    𝐜ᵀ𝐜 = 𝐜'*𝐜
+    D = 3𝐜*𝐜' - 𝐜ᵀ𝐜*I
+    A = kron(M̄,D)
+
+    B̄ = M̄[[1],:] #\int S̄ 
+    B = kron(B̄,I(3))
+
+    FBᵀ = -3*(𝐜ᵀ𝐜)*𝐜'*B
+    ## @show size(A), size(FBᵀ)
+    F = μ/𝐜ᵀ𝐜^(5/2)*(A*q+FBᵀ')
+end
+
+function gravitational_force_jacobian(nmcs::NC,m::T,Īₘ,r̄ₘ,q) where {T}
+    nld = get_num_of_local_dims(nmcs)
+    a = to_local_coords(nmcs,r̄ₘ)
+    z = Īg2z(nmcs,m,Īₘ,r̄ₘ)
+    M̄ = zeros(T,1+nld,1+nld)
+    M̄[1,1] = m
+    M̄[2:1+nld,1] = m*a
+    M̄[1,2:1+nld] = M̄[2:1+nld,1]
+    M̄[2:1+nld,2:1+nld] .= z
+    cₘ = to_local_coords(nmcs,r̄ₘ)
+    ## to_position
+    Cₘ = to_position_jacobian(nmcs,cₘ)
+    𝐜 = Cₘ*q
+    𝐜ᵀ𝐜 = 𝐜'*𝐜
+    D = 3𝐜*𝐜' - 𝐜ᵀ𝐜*I
+    A = kron(M̄,D)
+    ∂F∂q = μ/𝐜ᵀ𝐜^(5/2)*A
+end
+
+## Mass matrices: LACB
+
+function make_M_LACB(nmcs::NC,m::T,Īₘ,r̄ₘ) where {T} # ami (area moment of inertia tensor)
+    num_of_dim = get_num_of_dims(nmcs)
+    nld = get_num_of_local_dims(nmcs)
+    ncoords = get_num_of_coords(nmcs)
+    a = to_local_coords(nmcs,r̄ₘ)
+    z = Īg2z(nmcs,m,Īₘ,r̄ₘ)
+    M̄ = zeros(T,1+nld,1+nld)
+    M̄[1,1] = m
+    M̄[2:1+nld,1] = m*a
+    M̄[1,2:1+nld] = M̄[2:1+nld,1]
+    M̄[2:1+nld,2:1+nld] .= z
+    Id = IMatrix(num_of_dim)
+    M_std = kron(M̄,Id)
+    cv = nmcs.conversion_to_std
+    M = SMatrix{ncoords,ncoords}(transpose(cv)*M_std*cv)
+    B̄ = M̄[[1],:]
+    B = kron(B̄,Id)
+    M_LACB = [
+        m*Id B;
+        B' M;
+    ]
+    M_LACB[4:6,4:6] .+= Id
+    M_LACB
+end
+
+## Gravitational Potential: LACB
+function make_U_LACB(nmcs::NC,m::T,Īₘ,r̄ₘ,ro,q̂) where {T} # ami (area moment of inertia tensor)
+    num_of_dim = get_num_of_dims(nmcs)
+    nld = get_num_of_local_dims(nmcs)
+    ncoords = get_num_of_coords(nmcs)
+    cₘ = to_local_coords(nmcs,r̄ₘ)
+    z = Īg2z(nmcs,m,Īₘ,r̄ₘ)
+    M̄ = zeros(T,1+nld,1+nld)
+    M̄[1,1] = m
+    M̄[2:1+nld,1] = m*cₘ
+    M̄[1,2:1+nld] = M̄[2:1+nld,1]
+    M̄[2:1+nld,2:1+nld] .= z
+    ## M_std = kron(M̄,IMatrix(num_of_dim))
+    ## cv = nmcs.conversion_to_std
+    ## M = SMatrix{ncoords,ncoords}(transpose(cv)*M_std*cv)
+    B̄ = M̄[[1],:] #\int S̄ 
+    B = kron(B̄,I(3))
+    ## to_position
+    Cₘ = to_position_jacobian(nmcs,cₘ)
+    rc = Cₘ*q̂
+    𝐜 = ro + rc
+    𝐜ᵀ𝐜 = 𝐜'*𝐜
+    D = 3𝐜*𝐜' - 𝐜ᵀ𝐜*I
+
+    ## FBᵀ = -3*(𝐜ᵀ𝐜)*𝐜'*B
+
+    C = ro'*D*B
+  
+    A = kron(M̄,D)
+
+    ## d = 3*m*𝐜ᵀ𝐜*𝐜'*rc
+    
+    e = 1/2*m*ro'*D*ro
+    U_gra2 = -μ/𝐜ᵀ𝐜^(5/2)*(
+        1/2*q̂'*A*q̂ + 
+        C*q̂ + 
+        e
+    )
+    ## @show FBᵀ*q̂ + d
+    ## @show U_gra2
+    U_gra =  U_gra2
+end
+
+function gravitational_force_LACB(nmcs::NC,m::T,Īₘ,r̄ₘ,ro,q̂) where {T}
+    num_of_dim = get_num_of_dims(nmcs)
+    nld = get_num_of_local_dims(nmcs)
+    cₘ = to_local_coords(nmcs,r̄ₘ)
+    z = Īg2z(nmcs,m,Īₘ,r̄ₘ)
+    M̄ = zeros(T,1+nld,1+nld)
+    M̄[1,1] = m
+    M̄[2:1+nld,1] = m*cₘ
+    M̄[1,2:1+nld] = M̄[2:1+nld,1]
+    M̄[2:1+nld,2:1+nld] .= z
+    ## to_position
+    Cₘ = to_position_jacobian(nmcs,cₘ)
+    ĉ = Cₘ*q̂
+    𝐜 = ro + ĉ
+    𝐜ᵀ𝐜 = 𝐜'*𝐜
+    D = 3𝐜*𝐜' - 𝐜ᵀ𝐜*I
+    
+    Id = IMatrix(num_of_dim)
+    ## M_std = kron(M̄,Id)
+    ## cv = nmcs.conversion_to_std
+    ## M = SMatrix{ncoords,ncoords}(transpose(cv)*M_std*cv)
+    B̄ = M̄[[1],:] #\int S̄ 
+    B = kron(B̄,Id)
+    
+    ∂ĉᵀ∂q̂ᵀ = Cₘ'
+     
+    e = 1/2*m*ro'*D*ro
+
+    C = ro'*D*B
+
+    A = kron(M̄,D)
+
+    Bq̂ = B*q̂
+
+    ∂Cq̂∂oᵀ = (
+        3ro'*𝐜*I + 3ro*𝐜' + 3𝐜*𝐜' - 2𝐜*ro' - 𝐜ᵀ𝐜*I
+    )*Bq̂
+
+    ∂Cq̂∂ĉᵀ = (
+        3ro'*𝐜*I + 3ro*𝐜'        - 2𝐜*ro'
+    )*Bq̂
+
+    ∂e∂oᵀ = 3/2*m*(2(2ro+ĉ)*𝐜'*ro) - 1/2*m*(ro'*ro*2*𝐜 + 𝐜ᵀ𝐜*2*ro)
+    ∂e∂ĉᵀ = 3/2*m*(2( ro  )*𝐜'*ro) - 1/2*m*(ro'*ro*2*𝐜)
+
+    o = zero(𝐜)
+    ∂𝐜𝐜ᵀ∂x = hcat(𝐜,o,o) + vcat(𝐜',o',o')
+    ∂𝐜𝐜ᵀ∂y = hcat(o,𝐜,o) + vcat(o',𝐜',o')
+    ∂𝐜𝐜ᵀ∂z = hcat(o,o,𝐜) + vcat(o',o',𝐜')
+
+    ∂q̂ᵀAq̂∂oᵀ = 
+    ∂q̂ᵀAq̂∂ĉᵀ = 3*[
+        q̂'*kron(M̄,∂𝐜𝐜ᵀ∂x)*q̂,
+        q̂'*kron(M̄,∂𝐜𝐜ᵀ∂y)*q̂,
+        q̂'*kron(M̄,∂𝐜𝐜ᵀ∂z)*q̂,
+    ] + 2𝐜*q̂'*kron(M̄,Id)*q̂
+
+    ∂U∂q̂ᵀ = -μ/𝐜ᵀ𝐜^(5/2)*( 
+        A*q̂ + 
+        C' 
+    )
+
+    ∂U∂oᵀ = -μ/𝐜ᵀ𝐜^(5/2)*(
+            1/2*∂q̂ᵀAq̂∂oᵀ + 
+            ∂Cq̂∂oᵀ + 
+            ∂e∂oᵀ
+        ) + 5μ*𝐜/𝐜ᵀ𝐜^(7/2)*(
+            1/2*q̂'*A*q̂ + 
+            C*q̂ + 
+            e
+        )
+    
+    ∂U∂ĉᵀ = -μ/𝐜ᵀ𝐜^(5/2)*(
+            1/2*∂q̂ᵀAq̂∂ĉᵀ + 
+            ∂Cq̂∂ĉᵀ + 
+            ∂e∂ĉᵀ
+        ) + 5μ*𝐜/𝐜ᵀ𝐜^(7/2)*(
+            1/2*q̂'*A*q̂ + 
+            C*q̂ + 
+            e
+        )
+    ## ∂U∂qᵀ, ∂U∂oᵀ, ∂U∂ĉᵀ
+
+    vcat(
+        -∂U∂oᵀ,
+        -∂U∂q̂ᵀ - 
+         ∂ĉᵀ∂q̂ᵀ*∂U∂ĉᵀ
+    )
+end
+
+function gravitational_force_jacobian_LACB(nmcs::NC,m::T,Īₘ,r̄ₘ,𝐜) where {T}
+    nld = get_num_of_local_dims(nmcs)
+    a = to_local_coords(nmcs,r̄ₘ)
+    z = Īg2z(nmcs,m,Īₘ,r̄ₘ)
+    M̄ = zeros(T,1+nld,1+nld)
+    M̄[1,1] = m
+    M̄[2:1+nld,1] = m*a
+    M̄[1,2:1+nld] = M̄[2:1+nld,1]
+    M̄[2:1+nld,2:1+nld] .= z
+    𝐜ᵀ𝐜 = 𝐜'*𝐜
+    D = 3𝐜*𝐜' - 𝐜ᵀ𝐜*I
+    A = kron(M̄,D)
+    ## B̄ = M̄[:,1]
+    ## B = -3*(𝐜ᵀ𝐜)*kron(B̄,𝐜)
+    ∂f∂q = μ/𝐜ᵀ𝐜^(5/2)*A
+end
+
+
diff --git a/src/members/rigids/rigid_body.jl b/src/members/rigids/rigid_body.jl
index 9cfc8c0..3b261e8 100644
--- a/src/members/rigids/rigid_body.jl
+++ b/src/members/rigids/rigid_body.jl
@@ -233,6 +233,31 @@ function RigidBody(prop,state,coords,mesh=nothing)
     RigidBody(prop,state,coords,cache,mesh)
 end
 
+"""
+通用Rigid Body Type 
+$(TYPEDEF)
+---
+$(TYPEDFIELDS)
+"""
+struct GravitationalRigidBody{N,M,T,coordsType,cacheType,meshType} <: AbstractRigidBody{N,T}
+    "Rigid Body Property "
+    prop::RigidBodyProperty{N,T}
+    "Rigid Body State "
+    state::RigidBodyState{N,M,T}
+    "Coordinates State"
+    coords::NonminimalCoordinates{coordsType}
+    "Cache"
+    cache::cacheType
+    "Rigid Body Mesh"
+    mesh::meshType
+end
+
+function GravitationalRigidBody(prop,state,coords,mesh=nothing)
+    (;nmcs,pres_idx,cstr_idx) = coords
+    cache = BodyCache(prop,nmcs,pres_idx,cstr_idx)
+    GravitationalRigidBody(prop,state,coords,cache,mesh)
+end
+
 function body_state2coords_state(state::RigidBodyState,coords::NonminimalCoordinates)
     (;origin_frame) = state
     cartesian_frame2coords(coords.nmcs,origin_frame)
@@ -472,30 +497,83 @@ get_cstr_idx(::QCF.QC) = collect(1:1)
 
 # operations on rigid body
 """
-Return Rigid Body translational kinetic energy.
+Return Rigid Body kinetic energy.
 $(TYPEDSIGNATURES)
 """
-function kinetic_energy_translation(body::AbstractRigidBody)
+function kinetic_energy(body::AbstractRigidBody{3,T}) where {T}
     (;mass) = body.prop
-    (;mass_center_velocity) = body.state
-    T = 1/2*transpose(mass_center_velocity)*mass*mass_center_velocity
+    (;mass_locus_state) = body.state
+    mass_velocity = mass_locus_state.frame.velocity
+    Ω = to_3D(mass_locus_state.frame).local_angular_velocity
+    1/2*transpose(mass_velocity)*mass*mass_velocity + 1/2*transpose(Ω)*inertia*Ω
 end
 
 """
-Return Rigid Body rotational kinetic energy.
+Return Rigid Body kinetic energy.
 $(TYPEDSIGNATURES)
 """
-function kinetic_energy_rotation(body::AbstractRigidBody)
-    (;inertia) = body.prop
-    (;R,ω) = body.state
-    Ω = inv(R)*ω
-    T = 1/2*transpose(Ω)*inertia*Ω
+function kinetic_energy(body::AbstractRigidBody{2,T}) where T
+    (;prop,state) = body
+    (;mass,mass_locus,inertia) = prop
+    o = zero(T)
+    i = one(T)
+    M = Diagonal(mass.*ones(3))
+    c = vcat(
+        mass_locus.position, 
+        o
+    )
+    MS = M*skew(c)
+
+    I3 = SMatrix{3,3}(
+        [
+            inertia[1,1] inertia[1,2] o;
+            inertia[2,1] inertia[2,2] o;
+            o            o            inertia[1,1]+inertia[2,2];
+        ]
+    )
+
+    inertia_tensor = [
+         M  -MS;
+         MS  I3 + M*skew(c)*skew(c)'
+    ]
+
+    origin_frame_3d = CartesianFrame{3}(state.origin_frame)
+
+    R = origin_frame_3d.axes.X
+    Ω = origin_frame_3d.local_angular_velocity
+    v = origin_frame_3d.velocity
+    V = transpose(R)*v
+
+    η = vcat(
+        V,
+        Ω
+    )
+
+    0.5η'*inertia_tensor*η
+
+end
+
+function kinetic_energy(
+        coords::NonminimalCoordinates{<:NCF.NC},cache,
+        state::RigidBodyState,
+        prop::RigidBodyProperty,
+        q,q̇,
+    )
+    (;M) = cache.inertia_cache
+    T = 1/2*transpose(q̇)*M*q̇
 end
 
 """
 Return Rigid Body gravity potential energy.
 $(TYPEDSIGNATURES)
 """
+function potential_energy_gravity(body::RigidBody)
+    (;mass) = body.prop
+    (;mass_locus_state) = body.state
+    gravity_acceleration = get_gravity(body)
+    -transpose(mass_locus_state.frame.position)*gravity_acceleration*mass
+end
+
 function potential_energy_gravity(body::AbstractRigidBody)
     (;mass) = body.prop
     (;mass_locus_state) = body.state
diff --git a/src/postprocess/analysis.jl b/src/postprocess/analysis.jl
index 819a6f0..046e339 100644
--- a/src/postprocess/analysis.jl
+++ b/src/postprocess/analysis.jl
@@ -143,6 +143,45 @@ function get_angular_velocity!(bot::Robot,bodyid::Int,step_range=:)
     angular_velocity_traj[step_range] |> VectorOfArray
 end
 
+function get_kinetic_energy!(bot::Robot,bodyid::Int,step_range=:)
+    (; structure, traj)= bot
+	(; t, q, q̇) = traj
+	T = get_numbertype(bot)
+    N = get_num_of_dims(bot)
+    KE = [
+        zero(T)
+        for _ in eachindex(traj)
+    ]
+    bodies = get_bodies(structure)
+    body = bodies[bodyid]
+    for (i,(qₖ,q̇ₖ)) in enumerate(zip(q, q̇))
+        update_bodies!(structure,qₖ,q̇ₖ)
+        KE[i] = kinetic_energy(body,)
+    end
+    KE[step_range]
+end
+
+function get_kinetic_energy_coords!(bot::Robot,bodyid::Int,step_range=:)
+    (; structure, traj)= bot
+	(; t, q, q̇) = traj
+	T = get_numbertype(bot)
+    N = get_num_of_dims(bot)
+    KE = [
+        zero(T)
+        for _ in eachindex(traj)
+    ]
+    bodies = get_bodies(structure)
+    body = bodies[bodyid]
+    for (i,(qₖ,q̇ₖ)) in enumerate(zip(q, q̇))
+        update_bodies!(structure,qₖ,q̇ₖ)
+        KE[i] = kinetic_energy_coords(body,
+            structure.state.members[bodyid].q,
+            structure.state.members[bodyid].q̇,
+        )
+    end
+    KE[step_range]
+end
+
 function get_mid_times(bot::Robot)
     (;t) = bot.traj
     (t[begin+1:end] .+ t[begin:end-1])./2
diff --git a/src/postprocess/vis.jl b/src/postprocess/vis.jl
index 0d0745e..397dbc4 100644
--- a/src/postprocess/vis.jl
+++ b/src/postprocess/vis.jl
@@ -21,10 +21,12 @@ MakieCore.@recipe(Viz, structure) do scene
         refcolor=:lightgrey,
         cablewidth=0.6,
         meshcolor=:slategrey,
-        fontsize = 15,
+        fontsize = 12,
+        linewidth = 1,
     )
 end
 
+# AbstractBody
 function MakieCore.plot!(viz::Viz{Tuple{S}};
     ) where S <:AbstractBody
     body_ob = viz[:structure]
@@ -75,7 +77,7 @@ function MakieCore.plot!(viz::Viz{Tuple{S}};
             build_mesh(body_ob,color=viz.meshcolor[])
         end
         if viz.showwire[]
-            strokewidth = linewidth
+            strokewidth = viz.linewidth[]
         else
             strokewidth = 0
         end
@@ -83,13 +85,14 @@ function MakieCore.plot!(viz::Viz{Tuple{S}};
             MakieCore.mesh!(viz, meshes_ob; shading = MakieCore.Automatic())
         else
             MakieCore.poly!(viz, meshes_ob; shading = MakieCore.Automatic(),
-                # strokewidth
+                strokewidth
             )
         end
     end
     viz
 end
 
+# ClusterJoint
 function MakieCore.plot!(viz::Viz{Tuple{S}};
     ) where {S<:Apparatus{<:ClusterJoint}}
     cluster_cable_ob = viz[:structure]
@@ -112,6 +115,7 @@ function MakieCore.plot!(viz::Viz{Tuple{S}};
     end
 end
 
+# CableJoint
 function MakieCore.plot!(viz::Viz{Tuple{S}};
     ) where {S<:Apparatus{<:CableJoint}}
     cable_appar_ob = viz[:structure]
@@ -165,6 +169,7 @@ function MakieCore.plot!(viz::Viz{Tuple{S}};
     viz
 end
 
+# AbstractStructure
 function MakieCore.plot!(viz::Viz{Tuple{S}};
     ) where S <:AbstractStructure
     if viz.isref[]
@@ -245,7 +250,7 @@ function MakieCore.plot!(viz::Viz{Tuple{S}};
                 meshcolor,
                 pointcolor = viz.pointcolor[],
                 showmesh = viz.showmesh[],
-                # showwire = viz.showwire[],
+                showwire = viz.showwire[],
             )
         end
     end
@@ -286,18 +291,18 @@ end
 
 function make_patch(;trans=[0.0,0,0],rot=RotX(0.0),scale=1,color=:slategrey)
     parsedcolor = parse(CT.RGBA{Float32},color)
-    function patch(mesh)
+    function patch(mesh::GB.Mesh{Dim,T}) where {Dim,T}
         ct = Translation(trans) ∘ LinearMap(rot)
         updated_pos = ct.(mesh.position.*scale)
         fac = GB.faces(mesh)
-        nls = GB.normals(updated_pos,fac)
+        nls = GB.normals(updated_pos,fac;normaltype=GB.Vec{Dim,Float32})
         colors = fill(parsedcolor,length(updated_pos))
         GB.Mesh(GB.meta(updated_pos,normals=nls,color=colors),fac)
     end
 end
 
 function get_groundmesh(f::Function,rect)
-    GeometryBasics.Mesh(f, rect, NaiveSurfaceNets()) |> make_patch(;color = :snow)
+    GB.Mesh(f, rect,  Meshing.MarchingCubes()) |> make_patch(;color = :snow)
 end
 
 function get_groundmesh(plane::Plane,rect)
@@ -349,16 +354,16 @@ function get_groundmesh(::Nothing,rect)
 end
 
 function get_linesegs_cables(structure;slackonly=false,noslackonly=false)
-    (;connected) = structure.connectivity.tensioned
-    (;cables) = structure.apparatuses
+    cables = get_cables(structure)
     ndim = get_num_of_dims(structure)
     T = get_numbertype(structure)
-    linesegs_cables = Vector{Tuple{Point{ndim,T},Point{ndim,T}}}()
-    foreach(connected) do scnt
-        scable = cables[scnt.id]
-        ret = (Point(scnt.hen.body.state.loci_states[scnt.hen.pid].position),
-                Point(scnt.egg.body.state.loci_states[scnt.egg.pid].position))
-        slacking = scable.state.tension <= 0
+    linesegs_cables = Vector{Tuple{GB.Point{ndim,T},GB.Point{ndim,T}}}()
+    foreach(cables) do cable
+        force = cable.force
+        (;hen,egg) = cable.joint.hen2egg
+        ret = (GB.Point(hen.body.state.loci_states[hen.pid].frame.position),
+                GB.Point(egg.body.state.loci_states[egg.pid].frame.position))
+        slacking = force.state.tension <= 0
         if (slackonly && slacking) ||
            (noslackonly && !slacking) ||
            (!slackonly && !noslackonly)
diff --git a/src/structures/linearization.jl b/src/structures/linearization.jl
index 4de3cec..23fbdea 100644
--- a/src/structures/linearization.jl
+++ b/src/structures/linearization.jl
@@ -217,77 +217,77 @@ end
 
 function build_material_stiffness_matrix!(st::Structure,q,k)
     (;num_of_dim) = st
-    (;indexed,tensioned) = st.connectivity
+    (;indexed,) = st.connectivity
     (;num_of_full_coords,num_of_free_coords,sys_free_coords_idx,bodyid2sys_full_coords) = indexed
-    (;connected) = tensioned
-    (;cables) = st.apparatuses
     update!(st,q)
     Jj = zeros(eltype(q),num_of_dim,num_of_full_coords)
     retǨm = zeros(eltype(q),num_of_free_coords,num_of_free_coords)
-    foreach(connected) do scnt
-        j = scnt.id
-        rb1 = scnt.hen.body
-        rb2 = scnt.egg.body
-        ap1id = scnt.hen.pid
-        ap2id = scnt.egg.pid
-        C1 = rb1.cache.Cps[ap1id]
-        C2 = rb2.cache.Cps[ap2id]
-        mfull1 = bodyid2sys_full_coords[rb1.prop.id]
-        mfull2 = bodyid2sys_full_coords[rb2.prop.id]
-        cable = cables[j]
-        (;state) = cable
-        (;length,) = state
-        s = 1/length
-        Jj .= 0
-        Jj[:,mfull2] .+= C2
-        Jj[:,mfull1] .-= C1
-        Uj = transpose(Jj)*Jj
-        Ūjq = Uj[sys_free_coords_idx,:]*q
-        retǨm .+= k[j]*s^2*(Ūjq*transpose(Ūjq))
+    foreach(st.apparatuses) do appar
+        if appar.joint isa CableJoint
+            j = appar.id
+            (;force,joint) = appar
+            (;hen,egg) = joint.hen2egg
+            rb1 = hen.body
+            rb2 = egg.body
+            ap1id = hen.pid
+            ap2id = egg.pid
+            C1 = rb1.cache.Cps[ap1id]
+            C2 = rb2.cache.Cps[ap2id]
+            mfull1 = bodyid2sys_full_coords[rb1.prop.id]
+            mfull2 = bodyid2sys_full_coords[rb2.prop.id]
+            (;state) = force
+            (;length,) = state
+            s = 1/length
+            Jj .= 0
+            Jj[:,mfull2] .+= C2
+            Jj[:,mfull1] .-= C1
+            Uj = transpose(Jj)*Jj
+            Ūjq = Uj[sys_free_coords_idx,:]*q
+            retǨm .+= k[j]*s^2*(Ūjq*transpose(Ūjq))
+        end
     end
     retǨm
 end
 
 function build_geometric_stiffness_matrix!(st::Structure,q,f)
     (;num_of_dim) = st
-    (;indexed,tensioned) = st.connectivity
+    (;indexed,) = st.connectivity
     (;num_of_full_coords,num_of_free_coords,sys_free_coords_idx,bodyid2sys_full_coords) = indexed
-    (;connected) = tensioned
-    (;cables) = st.apparatuses
     update!(st,q)
     Jj = zeros(eltype(q),num_of_dim,num_of_full_coords)
     retǨg = zeros(eltype(q),num_of_free_coords,num_of_free_coords)
-    foreach(connected) do scnt
-        j = scnt.id
-        rb1 = scnt.hen.body
-        rb2 = scnt.egg.body
-        ap1id = scnt.hen.pid
-        ap2id = scnt.egg.pid
-        C1 = rb1.cache.Cps[ap1id]
-        C2 = rb2.cache.Cps[ap2id]
-        mfull1 = bodyid2sys_full_coords[rb1.prop.id]
-        mfull2 = bodyid2sys_full_coords[rb2.prop.id]
-        cable = cables[j]
-        (;state) = cable
-        (;length,) = state
-        s = 1/length
-        Jj .= 0
-        Jj[:,mfull2] .+= C2
-        Jj[:,mfull1] .-= C1
-        Uj = transpose(Jj)*Jj
-        Ǔj = @view Uj[sys_free_coords_idx,sys_free_coords_idx]
-        Ūjq = Uj[sys_free_coords_idx,:]*q
-        retǨg .+= f[j]/length*(Ǔj-s^2*Ūjq*transpose(Ūjq))
+    foreach(st.apparatuses) do appar
+        if appar.joint isa CableJoint
+            j = appar.id
+            (;force,joint) = appar
+            (;hen,egg) = joint.hen2egg
+            rb1 = hen.body
+            rb2 = egg.body
+            ap1id = hen.pid
+            ap2id = egg.pid
+            C1 = rb1.cache.Cps[ap1id]
+            C2 = rb2.cache.Cps[ap2id]
+            mfull1 = bodyid2sys_full_coords[rb1.prop.id]
+            mfull2 = bodyid2sys_full_coords[rb2.prop.id]
+            (;state) = force
+            (;length,) = state
+            s = 1/length
+            Jj .= 0
+            Jj[:,mfull2] .+= C2
+            Jj[:,mfull1] .-= C1
+            Uj = transpose(Jj)*Jj
+            Ǔj = @view Uj[sys_free_coords_idx,sys_free_coords_idx]
+            Ūjq = Uj[sys_free_coords_idx,:]*q
+            retǨg .+= f[j]/length*(Ǔj-s^2*Ūjq*transpose(Ūjq))
+        end
     end
     retǨg
 end
 
 function make_Ǩm_Ǩg(st,q0)
     (;num_of_dim) = st
-    (;numbered,indexed,tensioned) = st.connectivity
+    (;numbered,indexed,) = st.connectivity
     (;num_of_full_coords,num_of_free_coords,sys_pres_coords_idx,sys_free_coords_idx,bodyid2sys_full_coords) = indexed
-    (;connected) = tensioned
-    (;cables) = st.apparatuses
     (;bodyid2sys_loci_idx,sys_loci2coords_idx) = numbered
     function inner_Ǩm_Ǩg(q̌,s,μ,k,c)
 		q = Vector{eltype(q̌)}(undef,num_of_full_coords)
@@ -296,14 +296,15 @@ function make_Ǩm_Ǩg(st,q0)
         Jj = zeros(eltype(q̌),num_of_dim,num_of_full_coords)
         retǨm = zeros(eltype(q̌),num_of_free_coords,num_of_free_coords)
         retǨg = zeros(eltype(q̌),num_of_free_coords,num_of_free_coords)
-        foreach(connected) do scnt
-            j = scnt.id
-            rb1 = scnt.hen.body
-            rb2 = scnt.egg.body
+        foreach(st.apparatuses) do appar
+            j = appar.id
+            (;hen,egg) = appar.joint.hen2egg
+            rb1 = hen.body
+            rb2 = egg.body
             rb1id = rb1.prop.id
             rb2id = rb2.prop.id
-            ap1id = scnt.hen.pid
-            ap2id = scnt.egg.pid
+            ap1id = hen.pid
+            ap2id = egg.pid
             c1 = c[sys_loci2coords_idx[bodyid2sys_loci_idx[rb1id][ap1id]]]
             c2 = c[sys_loci2coords_idx[bodyid2sys_loci_idx[rb2id][ap2id]]]
             C1 = rb1.state.cache.funcs.C(c1)
@@ -328,18 +329,19 @@ function make_Ǩm_Ǩg(st,q0)
         Jj = zeros(eltype(q̌),num_of_dim,num_of_full_coords)
         retǨm = zeros(eltype(q̌),num_of_free_coords,num_of_free_coords)
         retǨg = zeros(eltype(q̌),num_of_free_coords,num_of_free_coords)
-        foreach(connected) do scnt
-            j = scnt.id
-            rb1 = scnt.hen.body
-            rb2 = scnt.egg.body
-            ap1id = scnt.hen.pid
-            ap2id = scnt.egg.pid
+        foreach(st.apparatuses) do appar
+            j = appar.id
+            (;force,joint) = appar
+            (;hen,egg) = joint.hen2egg
+            rb1 = hen.body
+            rb2 = egg.body
+            ap1id = hen.pid
+            ap2id = egg.pid
             C1 = rb1.state.cache.Cps[ap1id]
             C2 = rb2.state.cache.Cps[ap2id]
             mfull1 = bodyid2sys_full_coords[rb1.prop.id]
             mfull2 = bodyid2sys_full_coords[rb2.prop.id]
-            cable = cables[j]
-            (;k,c,state,slack) = cable
+            (;k,c,state,slack) = force
             (;direction,tension,length,lengthdot) = state
             s = 1/length
             Jj .= 0
@@ -357,10 +359,9 @@ end
 
 function make_S(st,q0)
     (;num_of_dim) = st
-    (;numbered,indexed,tensioned) = st.connectivity
+    (;numbered,indexed,) = st.connectivity
     (;sys_pres_coords_idx,sys_free_coords_idx,num_of_full_coords,bodyid2sys_full_coords) = indexed
     (;bodyid2sys_loci_idx,sys_loci2coords_idx) = numbered
-    (;connected) = tensioned
     (;cables) = st.apparatuses
     ncables = length(cables)
     function inner_S(q̌,s)
@@ -369,14 +370,16 @@ function make_S(st,q0)
 		q[sys_free_coords_idx] .= q̌
         ret = zeros(eltype(q̌),ncables)
         Jj = zeros(eltype(q̌),num_of_dim,num_of_full_coords)
-        foreach(connected) do scnt
-            j = scnt.id
-            rb1 = scnt.hen.body
-            rb2 = scnt.egg.body
+        foreach(st.apparatuses) do appar
+            j = appar.id
+            (;force,joint) = appar
+            (;hen,egg) = joint.hen2egg
+            rb1 = hen.body
+            rb2 = egg.body
             rb1id = rb1.prop.id
             rb2id = rb2.prop.id
-            ap1id = scnt.hen.pid
-            ap2id = scnt.egg.pid
+            ap1id = hen.pid
+            ap2id = egg.pid
             # c1 = c[sys_loci2coords_idx[bodyid2sys_loci_idx[rb1id][ap1id]]]
             # c2 = c[sys_loci2coords_idx[bodyid2sys_loci_idx[rb2id][ap2id]]]
             # C1 = rb1.state.cache.funcs.C(c1)
@@ -399,14 +402,15 @@ function make_S(st,q0)
         q[sys_free_coords_idx] .= q̌
         ret = zeros(eltype(q̌),ncables)
         Jj = zeros(eltype(q̌),num_of_dim,num_of_full_coords)
-        foreach(connected) do scnt
-            j = scnt.id
-            rb1 = scnt.hen.body
-            rb2 = scnt.egg.body
+        foreach(st.apparatuses) do appar
+            j = appar.id
+            (;hen,egg) = appar.joint.hen2egg
+            rb1 = hen.body
+            rb2 = egg.body
             rb1id = rb1.prop.id
             rb2id = rb2.prop.id
-            ap1id = scnt.hen.pid
-            ap2id = scnt.egg.pid
+            ap1id = hen.pid
+            ap2id = egg.pid
             c1 = c[sys_loci2coords_idx[bodyid2sys_loci_idx[rb1id][ap1id]]]
             c2 = c[sys_loci2coords_idx[bodyid2sys_loci_idx[rb2id][ap2id]]]
             C1 = rb1.state.cache.funcs.C(c1)
@@ -433,6 +437,11 @@ function build_tangent_stiffness_matrix!(∂Q̌∂q̌,appar::Apparatus,st::Abstr
     #default do nothing
 end
 
+
+function build_tangent_stiffness_matrix!(∂Q̌∂q̌,body::GravitationalRigidBody,st::AbstractStructure,q,s=nothing)
+    field_jacobian!(∂Q̌∂q̌,body,st)
+end
+
 function build_tangent_stiffness_matrix!(∂Q̌∂q̌,body::FlexibleBody,st::AbstractStructure,q,s=nothing)
     (;coords,cache) = body
     (;e,) = cache
@@ -756,7 +765,6 @@ function build_tangent_damping_matrix!(∂Q̌∂q̌̇,st)
     end
 end
 
-
 function init_cluster_matrix(st)
     (; bodies, apparatuses, connectivity) = st
     (; indexed, numbered) = connectivity
@@ -948,14 +956,17 @@ end
 
 function build_Ǩ(st)
     _,λ = check_static_equilibrium_output_multipliers(st)
-    build_Ǩ(st,λ)
+    q = get_coords(st)
+    build_Ǩ(st,q,λ)
 end
 
-function build_Ǩ(st,λ)
+function build_Ǩ(st,q,λ)
     (;num_of_free_coords) = st.connectivity.indexed
     T = get_numbertype(st)
-    # Ǩ = zeros(T,num_of_free_coords,num_of_free_coords)
-    Ǩ = -build_tangent_stiffness_matrix(st) .- cstr_forces_jacobian(st,λ)
+    ǨmǨg = zeros(T,num_of_free_coords,num_of_free_coords)
+    build_tangent_stiffness_matrix!(ǨmǨg,st)
+    Ǩa = cstr_forces_jacobian(st,q,λ)
+    Ǩ = - ǨmǨg .- Ǩa
     # Ǩ .= Ǩ
     Ǩ
 end
@@ -975,7 +986,7 @@ function undamped_eigen(st;gravity=false)
     q = get_coords(st)
     q̌ = get_free_coords(st)
     M̌ = assemble_M̌(st)
-    Ǩ = build_Ǩ(st,λ)
+    Ǩ = build_Ǩ(st,q,λ)
     Ǎ = make_cstr_jacobian(st)(q)
     Ň = nullspace(Ǎ)
     ℳ = transpose(Ň)*M̌*Ň
diff --git a/src/structures/mutate.jl b/src/structures/mutate.jl
index a88d753..029be87 100644
--- a/src/structures/mutate.jl
+++ b/src/structures/mutate.jl
@@ -227,7 +227,7 @@ function move_rigids!(st)
     (;bodies,state) = st
     foreach(bodies) do body
         bodyid = body.prop.id
-        (;q,q̇) = state.parts[bodyid]
+        (;q,q̇) = state.members[bodyid]
         update_loci_states!(body,q,q̇)
     end
 end
@@ -314,10 +314,71 @@ end
 """
 $(TYPEDSIGNATURES)
 """
-function apply_gravity!(st;factor=1)
-    (;bodies) = st
-    gravity_acceleration = factor*get_gravity(st)
+function apply_field!(st::AbstractStructure;factor=1)
+    (;bodies,state) = st
+    foreach(bodies) do body
+        (;id) = body.prop
+        state.members[id].q
+        apply_field!(
+            state.members[id].F,
+            body,
+            state.members[id].q;
+            factor
+        )
+    end
+end
+
+function apply_field!(F,body::RigidBody,q;factor=1)
+    gravity_acceleration = factor*get_gravity(body)
+    (;Cps,Cg,inertia_cache) = cache
+    f_gra = gravity_acceleration*body.prop.mass
+    mul!(F,transpose(Cg),f_gra,1,1)
+end
+
+function apply_field!(F,body::GravitationalRigidBody,q;factor=1)
+    nmcs = body.coords.nmcs
+    m = body.prop.mass
+    Ī = body.prop.inertia
+    r̄g = body.prop.mass_locus.position
+    rg = body.state.mass_locus_state.frame.position
+
+    F_gra = NCF.gravitational_force(nmcs,m,Ī,r̄g,rg,q)
+    ## @show size(F_gra)
+    F .+= F_gra
+end
+
+"""
+$(TYPEDSIGNATURES)
+"""
+function field_jacobian!(∂F∂q, st::AbstractStructure;factor=1)
+    (;bodies,state,connectivity) = st
+    (;
+        bodyid2sys_free_coords_idx,
+        bodyid2sys_full_coords_idx,
+    ) = connectivity.indexed
     foreach(bodies) do body
-        body.state.mass_locus_state.force .+= gravity_acceleration*body.prop.mass
+        (;id) = body.prop
+        state.members[id].q
+        field_jacobian!(
+            (@view ∂F∂q[bodyid2sys_free_coords_idx,bodyid2sys_free_coords_idx]),
+            body,
+            state.members[id].q;
+            factor
+        )
     end
 end
+
+function field_jacobian!(∂F̌∂q̌,body::RigidBody,q;factor=1)
+end
+
+function field_jacobian!(∂F̌∂q̌,body::GravitationalRigidBody,q;factor=1)
+    nmcs = body.coords.nmcs
+    m = body.prop.mass
+    Ī = body.prop.inertia
+    r̄g = body.prop.mass_locus.position
+    rg = body.state.mass_locus_state.frame.position
+
+    ## @show size(∂f∂q)
+    ∂F̌∂q̌ .+= NCF.gravitational_force_jacobian(nmcs,m,Ī,r̄g,rg)
+end
+
diff --git a/src/structures/structure.jl b/src/structures/structure.jl
index 0e4229f..0dd360f 100644
--- a/src/structures/structure.jl
+++ b/src/structures/structure.jl
@@ -39,16 +39,15 @@ function StructureState(bodies,apparatuses,cnt::Connectivity)
     end
     T = get_numbertype(bodies)
     t = zero(T)
-    q = Vector{T}(undef,num_of_full_coords)
+    q = zeros(T,num_of_full_coords)
     q̇ = zero(q)
     q̈ = zero(q)
     F = zero(q)
-    λ = Vector{T}(undef,num_of_cstr)
-    # s = Vector{T}(undef,num_of_add_var)
-    s = zeros(T, num_of_add_var)
-    c = get_local_coords(bodies,numbered)
+    λ = zeros(T,num_of_cstr)
+    s = zeros(T,num_of_add_var)
     p = zero(q)
-    p̌ = Vector{T}(undef,num_of_free_coords)
+    p̌ = zeros(T,num_of_free_coords)
+    c = get_local_coords(bodies,numbered)
     system = NonminimalCoordinatesState(t,q,q̇,q̈,F,p,p̌,λ,s,sys_free_coords_idx,sys_pres_coords_idx,c)
     members = [
         begin
@@ -131,7 +130,7 @@ function update!(st::AbstractStructure; gravity=false)
     update_bodies!(st)
     update_apparatuses!(st)
     if gravity
-        apply_gravity!(st)
+        apply_field!(st)
     end
     assemble_forces!(st)
 end
@@ -141,7 +140,7 @@ function lazy_update!(st::AbstractStructure; gravity=false)
     lazy_update_bodies!(st)
     update_apparatuses!(st)
     if gravity
-        apply_gravity!(st)
+        apply_field!(st)
     end
     assemble_forces!(st)
 end
@@ -516,8 +515,6 @@ function make_auxi_jacobian(structure::AbstractStructure,s0)
         # @show maximum(∂ζ∂q)
 
         ∂ζ∂s = reduce(blockdiag, A_list)
-        # @show Tb
-        # @show ∂ζ∂s
 
         ∂S∂q .= 1/2 * coζ*∂ζ∂q 
         ∂S∂s .= 1/2 *coζ*∂ζ∂s + cos̄*I
@@ -604,51 +601,6 @@ function make_cstr_jacobian(structure::AbstractStructure,q0::AbstractVector)
 end
 
 
-function build_ψ_hole(structure::AbstractStructure)
-    (;apparatuses) = structure
-    nholes = 0
-    nh = 0
-    FB = FischerBurmeister(1e-14, 10., 10.)
-    foreach(apparatuses) do appar
-        if isa(appar, Apparatus{<:ClusterJoint})
-            nholes += 1
-            nh += length(appar.joint.sps)
-        end
-    end
-    function _inner_ψ(s̄)
-        ψ = Vector{eltype(s̄)}(undef,2nh)
-        ψ⁺ = @view ψ[begin:2:end]
-        ψ⁻ = @view ψ[begin+1:2:end]
-        s⁺ = @view s̄[begin:2:end]
-        s⁻ = @view s̄[begin+1:2:end]
-        ih = 0
-        foreach(apparatuses) do appar
-            if isa(appar, Apparatus{<:ClusterJoint})
-                (;sps) = appar.joint
-                (;segs) = appar.force
-                nsi = length(sps)
-                for (i, sp) in enumerate(sps)
-                    Fₙ = norm(segs[i+1].force.state.force + segs[i].force.state.force)
-                    Fᵢ₊₁ = segs[i+1].force.state.tension
-                    Fᵢ = segs[i].force.state.tension
-                    ζ⁺ = sp.μ*Fₙ - Fᵢ₊₁ + Fᵢ
-                    ζ⁻ = Fᵢ₊₁ - Fᵢ + sp.μ*Fₙ
-                    ψ⁺[ih+i] = FB(ζ⁺, s⁺[ih+i])
-                    ψ⁻[ih+i] = FB(ζ⁻, s⁻[ih+i])
-                    # @show ζ⁺, ζ⁻
-                end
-                ih += nsi
-            end
-        end
-        ψ
-    end
-
-    function inner_ψ(s̄)
-        _inner_ψ(s̄)
-    end
-    inner_ψ
-end
-
 function build_F̌(st,bodyid,pid,f)
     T = get_numbertype(st)
     (;num_of_free_coords,bodyid2sys_free_coords) = st.connectivity.indexed
@@ -710,7 +662,7 @@ function check_jacobian_singularity(st)
             A_body = cstr_jacobian(body.coords,q_body)
             body_rank = rank(A_body)
             if body_rank < minimum(size(A_body))
-                @warn "The $(bodyid)th body's Jacobian is singular: rank(A(q))=$(body_rank)<$(minimum(size(A_rb)))"
+                @warn "The $(bodyid)th body's Jacobian is singular: rank(A(q))=$(body_rank)<$(minimum(size(A_body)))"
             end
         end
     end
diff --git a/test/Project.toml b/test/Project.toml
index faf8308..52f1b50 100644
--- a/test/Project.toml
+++ b/test/Project.toml
@@ -73,6 +73,7 @@ Revise = "295af30f-e4ad-537b-8983-00126c2a3abe"
 Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc"
 RowEchelon = "af85af4c-bcd5-5d23-b03a-a909639aa875"
 SafeTestsets = "1bc83da4-3b8d-516f-aca4-4fe02f6d838f"
+SatelliteToolbox = "6ac157d9-b43d-51bb-8fab-48bf53814f4a"
 Setfield = "efcf1570-3423-57d1-acb7-fd33fddbac46"
 SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf"
 StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
@@ -92,5 +93,4 @@ XLSX = "fdbf4ff8-1666-58a4-91e7-1b58723a45e0"
 Zygote = "e88e6eb3-aa80-5325-afca-941959d7151f"
 
 [compat]
-Makie = "0.20"
 julia = "1.9"
diff --git a/test/vis.jl b/test/vis.jl
index a107e66..c57ec89 100644
--- a/test/vis.jl
+++ b/test/vis.jl
@@ -170,11 +170,14 @@ function plot_traj!(bot::RB.Robot;
         xlims=(-1.0,1.0),
         ylims=(-1.0,1.0),
         zlims=(-1e-3,1.0),
+        show_axis = true,
         showground=true,
         ground=nothing,
         showback=false,
         actuate=false,
         figname=nothing,
+        rowgap=0,
+        colgap=0,
         showinit=false,
         auto=false,
         titleformatfunc = (subgrid_idx,tt)-> begin
@@ -207,7 +210,7 @@ function plot_traj!(bot::RB.Robot;
                 1 for subgrid_idx in eachindex(subgrids)
             ]
         (a,b::Nothing) => [
-                time2step(a[subgrid_idx],traj.t)
+                RB.time2step(a[subgrid_idx],traj.t)
                 for subgrid_idx in eachindex(subgrids)
             ]
         (a::Nothing,b) => b
@@ -244,44 +247,17 @@ function plot_traj!(bot::RB.Robot;
                 aspect=:data,
                 # tellheight=false,
             )
-            # scale = 1e3
-            # m0_yellow = load("yard/jixiebi/m0_yellow.STL") |> RB.make_patch(;rot=RotY(pi/2),trans=[-0.50, 0.0, 0.8],scale=1/scale,color=:yellow)
-            # m0_blue1 = load("yard/jixiebi/m0_blue.STL") |> RB.make_patch(;rot=RotY(pi/2), trans=[-0.5,0.23,0.37], scale=1/scale,color=:blue)
-            # m0_blue2 = load("yard/jixiebi/m0_blue.STL") |> RB.make_patch(;rot=RotY(pi/2), trans=[0.5,0.23,0.37], scale=1/scale,color=:blue)
-            # m0_blue3 = load("yard/jixiebi/m0_blue.STL") |> RB.make_patch(;rot=RotY(pi/2)*RotX(-pi/2), trans=[-0.23,0.5,0.37], scale=1/scale,color=:blue)
-            # m0_blue4 = load("yard/jixiebi/m0_blue.STL") |> RB.make_patch(;rot=RotY(pi/2)*RotX(-pi/2), trans=[-0.23,-0.5,0.37], scale=1/scale,color=:blue)
-            # mesh!(ax, m0_yellow)
-            # mesh!(ax, m0_blue1)
-            # mesh!(ax, m0_blue2)
-            # mesh!(ax, m0_blue3)
-            # mesh!(ax, m0_blue4)
-            # hidezdecorations!(ax)
-            # hideydecorations!(ax)
-
             xlims!(ax,xmin,xmax)
             ylims!(ax,ymin,ymax)
             zlims!(ax,zmin,zmax)
         elseif AxisType <: LScene
             showinfo = false
             # ax = LScene(fig[1,1], show_axis=false, scenekw = (clear=true,))
-            ax = LScene(fig[1,1],) #
-            cam = Makie.camera(ax.scene)
-            cam = cam3d!(ax.scene, projectiontype=Makie.Orthographic)
-            update_cam!(ax.scene, cam, pi/2, pi/8)
-            
-            # update_cam!(ax.scene, cam, pi/5, pi/12)
+            ax = LScene(fig[1,1], show_axis=show_axis, scenekw = (show_axis = show_axis,)) #
+            # cam = Makie.camera(ax.scene)
+            # cam = cam3d!(ax.scene, projectiontype=Makie.Orthographic)
+            # update_cam!(ax.scene, cam)
             # ax = LScene(fig[1,1],show_axis=false) #
-            # scale = 1e3
-            # m0_yellow = load("yard/jixiebi/m0_yellow.STL") |> RB.make_patch(;rot=RotY(pi/2),trans=[-0.50, 0.0, 0.8],scale=1/scale,color=:yellow)
-            # m0_blue1 = load("yard/jixiebi/m0_blue.STL") |> RB.make_patch(;rot=RotY(pi/2), trans=[-0.5,0.23,0.37], scale=1/scale,color=:blue)
-            # m0_blue2 = load("yard/jixiebi/m0_blue.STL") |> RB.make_patch(;rot=RotY(pi/2), trans=[0.5,0.23,0.37], scale=1/scale,color=:blue)
-            # m0_blue3 = load("yard/jixiebi/m0_blue.STL") |> RB.make_patch(;rot=RotY(pi/2)*RotX(-pi/2), trans=[-0.23,0.5,0.37], scale=1/scale,color=:blue)
-            # m0_blue4 = load("yard/jixiebi/m0_blue.STL") |> RB.make_patch(;rot=RotY(pi/2)*RotX(-pi/2), trans=[-0.23,-0.5,0.37], scale=1/scale,color=:blue)
-            # mesh!(ax, m0_yellow)
-            # mesh!(ax, m0_blue1)
-            # mesh!(ax, m0_blue2)
-            # mesh!(ax, m0_blue3)
-            # mesh!(ax, m0_blue4)
         else
             error("Unknown AxisType")
         end
@@ -348,7 +324,7 @@ function plot_traj!(bot::RB.Robot;
                     "fig. height" => map(string,ax.height),
                     "fig. width" => map(string,ax.width)
                 ]
-                if ndim == 3 && AxisType == Axis3
+                if AxisType == Axis3
                     cam_info = [
                         "azimuth" => map(string,ax.azimuth),
                         "elevation" => map(string,ax.elevation)
@@ -379,7 +355,7 @@ function plot_traj!(bot::RB.Robot;
                 framerate = 30 
                 skipstep = round(Int,1/framerate/dt*speedup)
                 recordsteps = 1:skipstep:length(traj.t)
-                record(fig, figname, recordsteps;
+                record(fig, figname, recordsteps; px_per_unit = 2,
                     framerate) do this_step
                     @show this_step / length(traj.t)
                     if actuate
@@ -422,15 +398,17 @@ function plot_traj!(bot::RB.Robot;
                 end
             end
         end
+        colgap!(grid1,colgap)
+        rowgap!(grid1,rowgap)
     end
-    if fig isa Figure
+    if (fig isa Figure) && !dorecord
         savefig(fig,figname)
         DataInspector(fig)
     end
     fig
 end
 
-function savefig(fig,figname=nothing;backend=GM)
+function savefig(fig,figname=nothing;backend=Makie.current_backend())
     if !isa(figname,Nothing)
         if isdefined(Main,:figdir)
             figpath = joinpath(figdir,figname)
diff --git a/yard/DualTriangles/static_stability.jl b/yard/DualTriangles/static_stability.jl
index 54e2701..94e9abf 100644
--- a/yard/DualTriangles/static_stability.jl
+++ b/yard/DualTriangles/static_stability.jl
@@ -103,7 +103,7 @@ function free(bot)
         RB.reset_forces!(st)
         RB.distribute_q_to_rbs!(st,q,q̇)
         RB.update_strings_apply_forces!(st)
-        # RB.apply_gravity!(st)
+        # RB.apply_field!(st)
         RB.assemble_forces!(F,st)
         # @show isapprox(F,Q̃*RB.fvector(st))
     end
diff --git a/yard/ES/main.jl b/yard/ES/main.jl
index de102e8..68a18a1 100644
--- a/yard/ES/main.jl
+++ b/yard/ES/main.jl
@@ -1916,7 +1916,7 @@ S,D = RB.static_kinematic_determine(ℬᵀ)
 ns = size(S,2)
 nk = size(D,2)
 
-λ = -inv(Ǎ*transpose(Ǎ))*Ǎ*Bᵀ*f
+λ = inv(Ǎ*transpose(Ǎ))*Ǎ*Bᵀ*f
 Ǩa = RB.cstr_forces_jacobian(bot.st,λ)
 𝒦a = transpose(Ň)*Ǩa*Ň |> Symmetric 
 vals_𝒦a,vecs_𝒦a = eigen(𝒦a)
diff --git a/yard/EWei/main.jl b/yard/EWei/main.jl
index f6b40b2..9514b17 100644
--- a/yard/EWei/main.jl
+++ b/yard/EWei/main.jl
@@ -1,47 +1,38 @@
-using LinearAlgebra
-using SparseArrays
-using StaticArrays
-using BenchmarkTools
-using FileIO, MeshIO
-using Printf
-using Makie
-import GLMakie as GM
-import CairoMakie as CM
-GM.activate!()
-using Unitful
-using NLsolve
-using Revise
-using StructArrays
-using EponymTuples
-using LaTeXStrings
-using StaticArrays
-using GeometryBasics
-using Rotations
-using CoordinateTransformations
-using Meshing
-using TypeSortedCollections
-using Match
+
+using Revise #jl
 import Rible as RB
-cd("examples/EWei")
-includet("define.jl")
-includet("../vis.jl")
+
+include(joinpath(pathof(RB),"../../yard/stability_stiffness.jl"))
+using AbbreviatedStackTraces #jl
+ENV["JULIA_STACKTRACE_ABBREVIATED"] = true #jl
+ENV["JULIA_STACKTRACE_MINIMAL"] = true #jl
+
+figdir::String = ""
+
+include(joinpath(pathof(RB),"../../test/vis.jl"))
+includet(joinpath(pathof(RB),"../../test/vis.jl")) #jl
+
+include(joinpath(pathof(RB),"../../examples/robots/EWei.jl"))
+includet(joinpath(pathof(RB),"../../examples/robots/EWei.jl")) #jl
+
 
 bot1 = BuildTail()
 
 bot1 = BuildTail(1; β=.9)
 
 # rigid body mesh
-RB.update!(bot1.st)
-RB.update_orientations!(bot1.st)
-rbs = RB.get_bodies(bot1.st)
-rbs[5].state.R
-rbs[5].state.cache.funcs.nmcs
-plot_rigid(rbs[2];showmesh=true,showupdatemesh=false)
+RB.update!(bot1.structure)
+rbs = RB.get_bodies(bot1.structure)
+rbs[5].state.origin_frame.axes
+rbs[5].coords.nmcs
+RB.viz(rbs[2];showmesh=true,showupdatemesh=false)
 
 # linearized dynamics
-ω², δq̌ = RB.undamped_eigen(bot1.st)
+ω², δq̌ = RB.undamped_eigen(bot1.structure)
 @show ω = [sqrt(ωi) for ωi in ω²[1:end]]
 
+plot_traj!(bot1)
+
 # nonlinear dynamics
 function dynfuncs(bot)
     (;st) = bot
@@ -50,7 +41,7 @@ function dynfuncs(bot)
         RB.update_bodies!(st,q,q̇)
         RB.distribute_s̄!(st,s)
         RB.update_apparatuses!(st)
-        ## RB.apply_gravity!(st)
+        ## RB.apply_field!(st)
         RB.assemble_forces!(st)
         RB.get_force!(F,st)
         ## F .= 0
diff --git a/yard/EWei/t2.jl b/yard/EWei/t2.jl
index 873981b..78324dc 100644
--- a/yard/EWei/t2.jl
+++ b/yard/EWei/t2.jl
@@ -312,7 +312,7 @@ bot = RB.Robot(st, hub)
 #         RB.update_bodies!(st,q,q̇)
 #         RB.distribute_s̄!(st,s)
 #         RB.update_apparatuses!(st)
-#         ## RB.apply_gravity!(st)
+#         ## RB.apply_field!(st)
 #         RB.assemble_forces!(st)
 #         RB.get_force!(F,st)
 #         ## F .= 0
diff --git a/yard/EWei/test1.jl b/yard/EWei/test1.jl
index fe61ed1..aac3fd3 100644
--- a/yard/EWei/test1.jl
+++ b/yard/EWei/test1.jl
@@ -128,7 +128,7 @@ plot_traj!(bot)
 #         RB.clear_forces!(st)
 #         RB.update_bodies!(st,q,q̇)
 #         RB.update_apparatuses!(st)
-#         # RB.apply_gravity!(st)
+#         # RB.apply_field!(st)
 #         RB.assemble_forces!(st)
 #         RB.get_force!(F,st)
 #         ## F .= 0
diff --git a/yard/LC/free_copy.jl b/yard/LC/free_copy.jl
index bcb85ae..334c5e3 100644
--- a/yard/LC/free_copy.jl
+++ b/yard/LC/free_copy.jl
@@ -40,7 +40,7 @@ function simulate_freevibra(num_of_dof;k,c,unit="mks",dt=0.01)
             RB.reset_forces!(st)
             RB.distribute_q_to_rbs!(st,q,q̇)
             RB.update_cables_apply_forces!(st)
-            # RB.apply_gravity!(st)
+            # RB.apply_field!(st)
             RB.assemble_forces!(F,st)
             # @show isapprox(F,Q̃*RB.fvector(st))
         end
diff --git a/yard/URDF/urdf.jl b/yard/URDF/urdf.jl
index 6dc38a5..9c73053 100644
--- a/yard/URDF/urdf.jl
+++ b/yard/URDF/urdf.jl
@@ -586,7 +586,7 @@ function dynfuncs(bot)
         RB.clear_forces!(st)
         RB.update_rigids!(st,q,q̇)
         # RB.update_apparatuses!(st)
-        RB.apply_gravity!(st)
+        RB.apply_field!(st)
         F .= RB.generate_forces!(st)
     end
     function Jac_F!(∂F∂q̌,∂F∂q̌̇,q,q̇,t)
diff --git a/yard/adjoint.jl b/yard/adjoint.jl
index 5ce2207..253e015 100644
--- a/yard/adjoint.jl
+++ b/yard/adjoint.jl
@@ -33,7 +33,8 @@ import GeometryBasics as GB
 using Makie
 import GLMakie as GM
 import CairoMakie as CM
-GM.activate!()
+import WGLMakie as WM
+WM.activate!()
 using LaTeXStrings
 using Meshing
 import Meshes
diff --git a/yard/adjoint/diffcontact.jl b/yard/adjoint/diffcontact.jl
new file mode 100644
index 0000000..302a680
--- /dev/null
+++ b/yard/adjoint/diffcontact.jl
@@ -0,0 +1,155 @@
+using Revise #jl
+import Rible as RB
+include(joinpath(pathof(RB),"../../yard/adjoint.jl"))
+
+using AbbreviatedStackTraces #jl
+ENV["JULIA_STACKTRACE_ABBREVIATED"] = true #jl
+ENV["JULIA_STACKTRACE_MINIMAL"] = true #jl
+
+figdir::String = joinpath(pathof(RB),"../../tmp")
+include(joinpath(pathof(RB),"../../test/vis.jl"))
+includet(joinpath(pathof(RB),"../../test/vis.jl")) #jl
+tw = 455.8843 #pt |> pt2px
+scalefactor = 4
+
+
+#--  cube
+include(joinpath(pathof(RB),"../../examples/robots/cube.jl"))
+includet(joinpath(pathof(RB),"../../examples/robots/cube.jl"))#jl
+
+include(joinpath(pathof(RB),"../../examples/robots/pointmass.jl"));
+includet(joinpath(pathof(RB),"../../examples/robots/pointmass.jl")) #jl
+
+#-- cube
+cubebot = make_cube()
+plot_traj!(cubebot;showwire=true)
+
+plane_env = RB.StaticContactSurfaces(
+    [
+        RB.HalfSpace([0,0,1.0],[0,0,0.0])
+    ]
+)
+
+ros = [[0,0,z] for z = 10.0:-0.1:0.0]
+qs = [
+    begin
+        cube = make_cube(origin_position,RotXY(π/4,π/4))
+        q = RB.get_coords(cube.structure)
+    end
+    for origin_position in ros
+]
+# p = RB.generalized_alpha(1.0)
+Ro = [
+    0.368869  -0.824063   0.429949;
+    0.769524   0.011314  -0.638518;
+    0.521314   0.566385   0.63831
+]
+origin_velocity = [0.0,0.0,0.0]
+ωo = [3.0,4.0,5.0]
+# cube = make_cube()
+# cube = make_cube(ros[1])
+cube = make_cube(ros[2],Ro,origin_velocity,0.1.*ωo)
+tspan = (0.0,1.91)
+tspan = (0.0,5.0)
+dt = 1e-3
+
+prob = RB.DynamicsProblem(
+    cube,
+    plane_env,
+    RB.RestitutionFrictionCombined(
+        RB.NewtonRestitution(),
+        RB.CoulombFriction(),
+    )
+)
+RB.solve!(prob,
+    RB.DynamicsSolver(
+        RB.Zhong06(),
+        RB.InnerLayerContactSolver(
+            RB.InteriorPointMethod()
+        )
+    );
+    tspan,dt,ftol=1e-10,maxiters=50,exception=false
+)
+RB.solve!(prob,
+    RB.DynamicsSolver(
+        RB.Zhong06(),
+        RB.MonolithicContactSolver(
+            RB.InteriorPointMethod()
+        )
+    );
+    tspan,dt,ftol=1e-10,maxiters=50,exception=false
+)
+
+plot_traj!(cube;
+    xlims = [-10,10],
+    ylims = [-10,10],
+    zlims = [-1e-3,10],
+    showinfo=false,
+    showwire=true
+)
+
+ME = RB.mechanical_energy!(cube)
+lines(cube.traj.t,ME.E)
+
+
+
+# ground plane
+θ=0.0
+ground_plane = RB.StaticContactSurfaces(
+    [
+        RB.HalfSpace([-tan(θ),0,1],zeros(3)),
+    ]
+);
+
+# time
+tspan = (0.0,1.5)
+h = 1e-3;
+
+# parameters and initial conditions
+restitution_coefficients = [0.5]
+v0s = [1.0];
+
+# pointmass
+pm = new_pointmass(;
+    e = restitution_coefficients[1],
+    μ=0.1,
+    origin_velocity = [v0s[1],0,0]
+);
+
+# frictional contact dynamics problem 
+prob = RB.DynamicsProblem(
+    pm,
+    ground_plane,
+    RB.RestitutionFrictionCombined(
+        RB.NewtonRestitution(),
+        RB.CoulombFriction(),
+    )
+);
+
+# simulate
+RB.solve!(
+    prob,
+    RB.DynamicsSolver(
+        RB.Zhong06(),
+        RB.MonolithicContactSolver(
+            RB.InteriorPointMethod()
+        )
+    );
+    tspan,dt=h,
+    ftol=1e-14,
+    maxiters=50,
+    exception=false
+);
+
+GM.activate!(;scalefactor=2); plot_traj!(
+    pm,
+    xlims=(-1e-3,1.0),
+    ylims=(-0.4,0.4),
+    zlims=(-1e-3,1.0),
+    showinfo =false,
+    showmesh=false,
+    showwire=false,
+    showlabels=false,
+    showcables=false,
+    showpoints=true,
+)
\ No newline at end of file
diff --git a/yard/gravitational/cube.jl b/yard/gravitational/cube.jl
new file mode 100644
index 0000000..476825e
--- /dev/null
+++ b/yard/gravitational/cube.jl
@@ -0,0 +1,176 @@
+using Revise #jl
+import Rible as RB
+include(joinpath(pathof(RB),"../../yard/tensegrity.jl"))
+
+using ForwardDiff
+import SatelliteToolbox as ST
+
+using AbbreviatedStackTraces #jl
+ENV["JULIA_STACKTRACE_ABBREVIATED"] = true #jl
+ENV["JULIA_STACKTRACE_MINIMAL"] = true #jl
+
+figdir::String = joinpath(pathof(RB),"../../tmp")
+include(joinpath(pathof(RB),"../../test/vis.jl"))
+includet(joinpath(pathof(RB),"../../test/vis.jl")) #jl
+tw = 455.8843 #pt |> pt2px
+scalefactor = 4
+
+
+#--  cube
+include(joinpath(pathof(RB),"../../examples/robots/cube.jl"))
+includet(joinpath(pathof(RB),"../../examples/robots/cube.jl"))#jl
+
+include(joinpath(pathof(RB),"../../examples/robots/pointmass.jl"));
+includet(joinpath(pathof(RB),"../../examples/robots/pointmass.jl")) #jl
+
+#-- cube
+cubebot = make_cube()
+plot_traj!(cubebot;showwire=true)
+
+plane_env = RB.StaticContactSurfaces(
+    [
+        RB.HalfSpace([0,0,1.0],[0,0,0.0])
+    ]
+)
+
+ros = [[0,0,z] for z = 10.0:-0.1:0.0]
+qs = [
+    begin
+        cube = make_cube(origin_position,RotXY(π/4,π/4))
+        q = RB.get_coords(cube.structure)
+    end
+    for origin_position in ros
+]
+
+Ro = [
+    0.368869  -0.824063   0.429949;
+    0.769524   0.011314  -0.638518;
+    0.521314   0.566385   0.63831
+]
+origin_velocity = [0.0,0.0,0.0]
+ωo = [3.0,4.0,5.0]
+# cube = make_cube()
+# cube = make_cube(ros[1])
+cube = make_cube(ros[2],Ro,origin_velocity,0.1.*ωo)
+tspan = (0.0,1.91)
+tspan = (0.0,5.0)
+dt = 1e-3
+
+prob = RB.DynamicsProblem(
+    cube,
+    plane_env,
+    RB.RestitutionFrictionCombined(
+        RB.NewtonRestitution(),
+        RB.CoulombFriction(),
+    )
+)
+RB.solve!(prob,
+    RB.DynamicsSolver(
+        RB.Zhong06(),
+        RB.InnerLayerContactSolver(
+            RB.InteriorPointMethod()
+        )
+    );
+    tspan,dt,ftol=1e-10,maxiters=50,exception=false
+)
+
+
+ME = RB.mechanical_energy!(cube)
+lines(cube.traj.t,ME.E)
+
+iss_tle = ST.tle"""
+       ISS (ZARYA)
+       1 25544U 98067A   23138.86946505 -.00404279  00000+0 -74572-2 0  9994
+       2 25544  51.6431 113.8899 0006661 357.1286  88.0982 15.49924990397244"""
+orbp = ST.Propagators.init(Val(:SGP4), iss_tle)
+ST.Propagators.propagate_to_epoch!(orbp, ST.date_to_jd(2023, 5, 19, 12, 0, 0))
+kepler_elems = ST.Propagators.mean_elements(orbp)
+
+orbital_period = ST.orbital_period(kepler_elems)
+
+ro, vo = ST.Propagators.mean_elements(orbp) |> ST.kepler_to_rv
+ro
+vo
+
+Ro = [
+    0.368869  -0.824063   0.429949;
+    0.769524   0.011314  -0.638518;
+    0.521314   0.566385   0.63831
+]
+ωo = [3.0,4.0,5.0]
+
+cube = make_cube(ro,Ro,vo,0.1.*ωo)
+
+tspan = (0.0,orbital_period)
+dt = 1.0
+
+prob = RB.DynamicsProblem(
+    cube,
+    RB.GravitationalEnv(),
+    ## RB.EmptyEnv()
+)
+
+RB.solve!(prob,
+    RB.DynamicsSolver(
+        RB.Zhong06(),
+    );
+    dt,
+    tspan,# = (0.0,2dt),
+    ftol=1e-9,maxiters=50,verbose=true,exception=true
+)
+
+plot_traj!(cube;
+    xlims = [-10,10],
+    ylims = [-10,10],
+    zlims = [-1e-3,10],
+    showinfo=false,
+    showwire=true
+)
+
+r1 = RB.get_trajectory!(cube,1,1)
+lines(Matrix(r1))
+
+q = RB.get_coords(cube.structure)
+body = RB.get_bodies(cube)[1]
+nmcs = body.coords.nmcs
+m = body.prop.mass
+Īₘ = body.prop.inertia
+r̄ₘ = body.prop.mass_locus.position
+rₘ = body.state.mass_locus_state.frame.position
+
+U_gra = RB.NCF.make_U(nmcs,m,Īₘ,r̄ₘ,q)
+F_gra = RB.NCF.gravitational_force(nmcs,m,Īₘ,r̄ₘ,q)
+∂F∂q_gra = RB.NCF.gravitational_force_jacobian(nmcs,m,Īₘ,r̄ₘ,q)
+cond(∂F∂q_gra)
+
+M = RB.NCF.make_M(nmcs,m,Īₘ,r̄ₘ)
+
+re = q[1:3] + [1000.0,2000.0,3000.0]
+rc = rₘ - re 
+q̂ = copy(q)
+q̂[1:3] .-= re
+q̂
+
+qs = vcat(re,q̂)
+
+U_gra = RB.NCF.make_U_LACB(nmcs,m,Īₘ,r̄ₘ,re,q̂)
+
+F_gra_fd(_x) = ForwardDiff.gradient((x)-> RB.NCF.make_U_LACB(nmcs,m,Īₘ,r̄ₘ,x[1:3],x[4:end]),_x)
+F_gra_Auto = F_gra_fd(qs)
+
+F_gra_LACB = RB.NCF.gravitational_force_LACB(nmcs,m,Īₘ,r̄ₘ,re,q̂)
+
+F_gra_Auto + F_gra_LACB
+
+∂F∂q_gra_fd(_x) = ForwardDiff.jacobian(F_gra_fd,_x)
+
+∂F∂q_gra_LACB = ∂F∂q_gra_fd(qs)
+
+cond(∂F∂q_gra_fd(qs))
+
+M_LACB = RB.NCF.make_M_LACB(nmcs,m,Īₘ,r̄ₘ)
+
+
+rank(M)
+cond(Array(M))
+
diff --git a/yard/jixiebi/Group2_1.jl b/yard/jixiebi/Group2_1.jl
index 7b3f74e..ea8c419 100644
--- a/yard/jixiebi/Group2_1.jl
+++ b/yard/jixiebi/Group2_1.jl
@@ -34,7 +34,7 @@ rigid_contacts = RB.ContactRigidBodies(
         # RB.RigidCube(14)
         # RB.RigidSphere(14)
     ],
-    gravity=RB.NoGravity()
+    gravity=RB.NoField()
 )
 
 # parameters
diff --git a/yard/jixiebi/dyfun.jl b/yard/jixiebi/dyfun.jl
index 2618efc..0a2cda5 100644
--- a/yard/jixiebi/dyfun.jl
+++ b/yard/jixiebi/dyfun.jl
@@ -5,7 +5,7 @@ function dynfuncs1(bot)
         TR.update_rigids!(tg,q,q̇)
         TR.distribute_s̄!(tg,s)
         TR.update_tensiles!(tg)
-        # TR.apply_gravity!(tg)
+        # TR.apply_field!(tg)
         TR.generate_forces!(tg)
         TR.get_force!(F,tg)
     end
diff --git a/yard/links/dynamics.jl b/yard/links/dynamics.jl
index 8cad4f9..9b51f79 100644
--- a/yard/links/dynamics.jl
+++ b/yard/links/dynamics.jl
@@ -51,7 +51,7 @@ function simulate_linkn(;dt=0.01,k=3e1,c=0.0,tend=10.0,verbose=false)
             RB.reset_forces!(st)
             RB.distribute_q_to_rbs!(st,q,q̇)
             RB.update_strings_apply_forces!(st)
-            RB.apply_gravity!(st)
+            RB.apply_field!(st)
             # F .= Q̃*RB.fvector(st)
             RB.assemble_forces!(F,st)
         end
diff --git a/yard/links/link2.jl b/yard/links/link2.jl
index fc653b9..0995762 100644
--- a/yard/links/link2.jl
+++ b/yard/links/link2.jl
@@ -202,7 +202,7 @@ function inverse(tgstruct,refstruct)
             RB.reset_forces!(tgstruct)
             RB.actuate!(tgstruct,u)
             RB.update_strings_apply_forces!(tgstruct)
-            RB.apply_gravity!(tgstruct)
+            RB.apply_field!(tgstruct)
             F .= 0
             #F .= Q̃*RB.fvector(tgstruct)
             RB.assemble_forces!(F,tgstruct)
@@ -241,7 +241,7 @@ function linearload(st,q0,initial_u,target_u,target_t)
         RB.update_strings_apply_forces!(st)
         # tensions = [s.state.tension for s in st.strings]
         # @show tensions
-        RB.apply_gravity!(st)
+        RB.apply_field!(st)
         #F .= Q̃*RB.fvector(st)
         F .= 0
         RB.assemble_forces!(F,st)
diff --git a/yard/manipulator/controller_Lya.jl b/yard/manipulator/controller_Lya.jl
index a332584..da8211d 100644
--- a/yard/manipulator/controller_Lya.jl
+++ b/yard/manipulator/controller_Lya.jl
@@ -42,7 +42,7 @@ function free(bot)
         RB.reset_forces!(st)
         RB.distribute_q_to_rbs!(st,q,q̇)
         RB.update_cables_apply_forces!(st)
-        RB.apply_gravity!(st)
+        RB.apply_field!(st)
         RB.assemble_forces!(F,st)
         # @show isapprox(F,Q̃*RB.fvector(st))
     end
diff --git a/yard/manipulator/eigen.jl b/yard/manipulator/eigen.jl
index 756d8c2..ca75975 100644
--- a/yard/manipulator/eigen.jl
+++ b/yard/manipulator/eigen.jl
@@ -137,7 +137,7 @@ function freevibra(tgstruct,q0)
         RB.distribute_q_to_rbs!(tgstruct,q,q̇)
         RB.update_cables_apply_forces!(tgstruct)
         # F .= Q̃*RB.fvector(tgstruct)
-        # RB.apply_gravity!(tgstruct,factor=0.01)
+        # RB.apply_field!(tgstruct,factor=0.01)
         # F .= G
         RB.assemble_forces!(F,tgstruct)
         # @show isapprox(F,Q̃*RB.fvector(tgstruct))
diff --git a/yard/manipulator/free.jl b/yard/manipulator/free.jl
index 9d554a4..aa950c2 100644
--- a/yard/manipulator/free.jl
+++ b/yard/manipulator/free.jl
@@ -48,7 +48,7 @@ function simulate_free(num_of_dof;k,c,unit="mks",dt=0.01,tend=10.0,verbose=false
             RB.reset_forces!(st)
             RB.distribute_q_to_rbs!(st,q,q̇)
             RB.update_cables_apply_forces!(st)
-            RB.apply_gravity!(st)
+            RB.apply_field!(st)
             RB.assemble_forces!(F,st)
             # @show isapprox(F,Q̃*RB.fvector(st))
         end
diff --git a/yard/manipulator/man_vibra.jl b/yard/manipulator/man_vibra.jl
index 8b70db9..de66bc9 100644
--- a/yard/manipulator/man_vibra.jl
+++ b/yard/manipulator/man_vibra.jl
@@ -40,7 +40,7 @@ function dynfuncs(tgstruct,q0)
         RB.reset_forces!(tgstruct)
         # RB.distribute_q_to_rbs!(tgstruct,q,q̇)
         # RB.update_cables_apply_forces!(tgstruct)
-        RB.apply_gravity!(tgstruct)
+        RB.apply_field!(tgstruct)
         #F .= Q̃*RB.fvector(tgstruct)
         F .= 0.0
         RB.assemble_forces!(F,tgstruct)
diff --git a/yard/manipulator/planning.jl b/yard/manipulator/planning.jl
index 9d8270d..256da24 100644
--- a/yard/manipulator/planning.jl
+++ b/yard/manipulator/planning.jl
@@ -73,7 +73,7 @@ function dynamics(tr,action)
         RB.reset_forces!(st)
         RB.distribute_q_to_rbs!(st,q,q̇)
         RB.update_cables_apply_forces!(st)
-        RB.apply_gravity!(st;factor=0.1)
+        RB.apply_field!(st;factor=0.1)
         RB.assemble_forces!(F,st)
     end
     Jac_Γ = RB.build_Jac_Γ(st)
diff --git a/yard/manipulator/static_stability.jl b/yard/manipulator/static_stability.jl
index c2455ea..ca1cc9c 100644
--- a/yard/manipulator/static_stability.jl
+++ b/yard/manipulator/static_stability.jl
@@ -95,7 +95,7 @@ function free(bot)
         RB.reset_forces!(st)
         RB.distribute_q_to_rbs!(st,q,q̇)
         RB.update_cables_apply_forces!(st)
-        # RB.apply_gravity!(st)
+        # RB.apply_field!(st)
         RB.assemble_forces!(F,st)
         # @show isapprox(F,Q̃*RB.fvector(st))
     end
diff --git a/yard/ming/ming.jl b/yard/ming/ming.jl
index 2d66bbd..8b96896 100644
--- a/yard/ming/ming.jl
+++ b/yard/ming/ming.jl
@@ -171,7 +171,7 @@ function simulate_linkn(linkn;dt=0.01,tend=10.0,target_temps=[40,40,40],verbose=
             RB.distribute_q_to_rbs!(st,q,q̇)
             RB.update_strings_apply_forces!(st)
             RB.update_SMA_strings_apply_forces!(st)
-            RB.apply_gravity!(st)
+            RB.apply_field!(st)
             # F .= Q̃*RB.fvector(st)
             # rb3 = st.rigidbodies[end]
             # f = [5.0,0.0,0.0]
diff --git a/yard/ming/verify.jl b/yard/ming/verify.jl
index 8095a9c..87c597b 100644
--- a/yard/ming/verify.jl
+++ b/yard/ming/verify.jl
@@ -113,7 +113,7 @@ function dynfuncs(tgbot)
         RB.reset_forces!(st)
         RB.distribute_q_to_rbs!(st,q,q̇)
         RB.update_strings_apply_forces!(st)
-        # RB.apply_gravity!(st)
+        # RB.apply_field!(st)
         RB.assemble_forces!(F,st)
     end
     M,Φ,A,F!,nothing
diff --git a/yard/nonsmooth/cube.jl b/yard/nonsmooth/cube.jl
index 1d2508c..d66fa6b 100644
--- a/yard/nonsmooth/cube.jl
+++ b/yard/nonsmooth/cube.jl
@@ -1,55 +1,5 @@
 
 #-- cube
-function make_cube(origin_position = [0.0,0.0,0.5],
-    R = one(RotMatrix{3}),
-    origin_velocity = [2.0,0.0,0.0],
-    ω = [0.0,0.0,5.0];
-    μ = 0.9,
-    e = 0.0
-)
-m = 1.0
-mass_locus = zeros(3)
-inertia = SMatrix{3,3}(Matrix(1.0I,3,3))
-h = 5.0
-loci = [h.*[x,y,z] for z = [-1,1] for y = [-1,1] for x = [-1,1]]
-axes = [SVector(1.0,0,0) for _ = 1:length(loci)]
-friction_coefficients = [μ for _ = 1:length(loci)]
-restitution_coefficients = [e for _ = 1:length(loci)]
-pts = Point3.(loci)
-fcs = GB.QuadFace.([
-    [1,3,4,2],
-    [3,7,8,4],
-    [7,5,6,8],
-    [5,1,2,6],
-    [2,4,8,6],
-    [1,5,7,3]
-])
-nls = GB.normals(pts,fcs)
-cube_mesh = GB.Mesh(GB.meta(pts,normals=nls),fcs)
-prop = RB.RigidBodyProperty(
-    1,true,m,inertia,
-    mass_locus,loci,axes,
-    friction_coefficients,restitution_coefficients,
-    )
-ri = copy(origin_position)
-nmcs = RB.NCF.NC1P3V(ri,origin_position,R)
-state = RB.RigidBodyState(prop,origin_position,R,origin_velocity,ω)
-rb1 = RB.RigidBody(prop,state,cube_mesh)
-rbs = TypeSortedCollection((rb1,))
-numbered = RB.number(rbs)
-matrix_sharing = zeros(Int,0,0)
-indexed = RB.index(rbs,matrix_sharing)
-ss = Int[]
-apparatuses = (cables = ss,)
-hub = nothing
-#
-connected = RB.connect(rbs,zeros(Int,0,0))
-tensioned = @eponymtuple(connected)
-cnt = RB.Connectivity(numbered,indexed,tensioned)
-st = RB.Structure(rbs,apparatuses,cnt,)
-bot = RB.Robot(st,nothing)
-end
-
 cubebot = make_cube()
 plot_traj!(cubebot)
 
diff --git a/yard/nonsmooth/make_notebooks.jl b/yard/nonsmooth/make_notebooks.jl
index 9415ee1..76ce989 100644
--- a/yard/nonsmooth/make_notebooks.jl
+++ b/yard/nonsmooth/make_notebooks.jl
@@ -28,20 +28,20 @@ using GLMakie
 # )
 
 
-Literate.notebook(
-    joinpath(@__DIR__,"meteor_hammer.jl"), 
-    joinpath(@__DIR__,"../../../notebooks/");
-    name="meteor_hammer",
-    execute = true,
-    # documenter = false,
-)
-
-
 ## Literate.notebook(
-##     joinpath(@__DIR__,"superball.jl"), 
+##     joinpath(@__DIR__,"meteor_hammer.jl"), 
 ##     joinpath(@__DIR__,"../../../notebooks/");
-##     name="superball",
+##     name="meteor_hammer",
 ##     execute = true,
 ##     # documenter = false,
 ## )
+
+
+Literate.notebook(
+    joinpath(@__DIR__,"superball.jl"), 
+    joinpath(@__DIR__,"../../../notebooks/");
+    name="superball",
+    execute = true,
+    # documenter = false,
+)
 GLMakie.closeall()
\ No newline at end of file
diff --git a/yard/nonsmooth/superball.jl b/yard/nonsmooth/superball.jl
index ab2dcc5..e8c0f32 100644
--- a/yard/nonsmooth/superball.jl
+++ b/yard/nonsmooth/superball.jl
@@ -48,9 +48,11 @@ ballbot = superball(
     e = 0.8,
     l,d,
     z0 = l^2/(sqrt(5)*d) + 2.0,
+    visible = true,
     constrained = false,
     loadmesh = false,
 )
+
 # time 
 tspan = (0.0,5.0)
 h = 1e-2
@@ -125,7 +127,7 @@ GM.activate!(;scalefactor); with_theme(theme_pub;
                 meshcolor = Makie.RGBAf(r,g,b,alphas[i]))
             end
             RB.hidey(ax)
-            lines!(ax,r2p1)
+            lines!(ax,Matrix(r2p1))
         end
     )
     ax31 = Axis(gd2[1,1],
@@ -189,6 +191,37 @@ GM.activate!(;scalefactor); with_theme(theme_pub;
     fig
 end
 
+GM.activate!(;scalefactor=4,px_per_unit=4); with_theme(theme_pub;
+        figure_padding = (0,0,0,-fontsize),
+        size = (1.0tw,0.30tw),
+        Axis3 = (
+            azimuth = 4.7855306333269805,
+            elevation = 0.03269908169872391,
+            xlabeloffset = fontsize,
+            zlabeloffset = 0.6fontsize,
+        )
+    ) do
+    plot_traj!(ballbot;
+        AxisType = Axis3,
+        figname = "ballbot_rolling.gif",
+        xlims = [-1,20],
+        ylims = [-1,8],
+        zlims = [-1e-3,3.0],
+        doslide = false,
+        dorecord = true,
+        showinfo = false,
+        showpoints = false,
+        showlabels = false,
+        showmesh = true,
+        showwire = true,
+        showtitle = false,
+        showcables  = true,
+        sup! = (ax,_,_) -> begin
+            RB.hidey(ax)
+        end
+    )
+end
+
 # ## Second Scenario
 ballbot = superball(
     0.0;
@@ -272,7 +305,7 @@ GM.activate!(;scalefactor); with_theme(theme_pub;
                 cablecolor=Makie.RGBAf(db.r,db.g,db.b,Makie.N0f8(alphas[i])),
                 meshcolor = Makie.RGBAf(r,g,b,alphas[i]))
             end
-            lines!(ax,r2p1)
+            lines!(ax,Matrix(r2p1))
         end
     )
     ax2 = Axis(gd2[1,1];tellheight=false,xlabel=tlabel,ylabel = "Energy (J)")
@@ -295,6 +328,32 @@ GM.activate!(;scalefactor); with_theme(theme_pub;
     fig
 end
 
+GM.activate!(;scalefactor=4,px_per_unit=4); with_theme(theme_pub;
+        size = (0.6tw,0.30tw),
+        figure_padding = (-2fontsize,0.5fontsize,0,0),
+        Axis3 = (
+            azimuth = 4.825530633326982,
+            elevation = 0.6726990816987243
+        )
+    ) do
+    plot_traj!(ballbot;
+        AxisType = Axis3,
+        figname="ballbot_sliding.gif",
+        xlims = [-1,6],
+        ylims = [-1,3],
+        zlims = [-1e-3,1.55],
+        doslide = false,
+        dorecord = true,
+        showinfo = false,
+        showpoints = false,
+        showlabels = false,
+        showtitle = false,
+        showcables = true,
+        showmesh = true,
+        showwire = false,
+    )
+end
+
 step_start = RB.time2step(1.6,ballbot.traj.t) #src
 step_stop = RB.time2step(2.5,ballbot.traj.t) #src
 r2p1 = RB.get_trajectory!(ballbot,2,1) #src
@@ -325,7 +384,7 @@ stats_superballs_dt = [
             );
             tspan=(0.0,0.1),dt,ftol=ifelse(dt==5e-6,1e-14,1e-12),
             maxiters=500,exception=false
-        ).prob.bot
+        )[1].prob.bot
     end
     for dt in dts, solver in (RB.Zhong06(),RB.Moreau(0.5))
 ];
diff --git a/yard/spine3d/main.jl b/yard/spine3d/main.jl
index 7cae1ad..cfe1e73 100644
--- a/yard/spine3d/main.jl
+++ b/yard/spine3d/main.jl
@@ -184,7 +184,7 @@ function dynfuncs(bot)
         RB.reset_forces!(st)
         RB.distribute_q_to_rbs!(st,q,q̇)
         RB.update_cables_apply_forces!(st)
-        # RB.apply_gravity!(st,factor=0.001)
+        # RB.apply_field!(st,factor=0.001)
         # F .= Q̃*RB.fvector(st)
         F .= 0.0
         RB.assemble_forces!(F,st)
diff --git a/yard/stiffness/deps.jl b/yard/stability_stiffness.jl
similarity index 100%
rename from yard/stiffness/deps.jl
rename to yard/stability_stiffness.jl
diff --git a/yard/stiffness/Tbars.jl b/yard/stiffness/Tbars.jl
index c85090d..0b94cd3 100644
--- a/yard/stiffness/Tbars.jl
+++ b/yard/stiffness/Tbars.jl
@@ -1,23 +1,28 @@
-include("deps.jl")
+
+using Revise #jl
+import Rible as RB
+
+include(joinpath(pathof(RB),"../../yard/stability_stiffness.jl"))
 using AbbreviatedStackTraces #jl
 ENV["JULIA_STACKTRACE_ABBREVIATED"] = true #jl
 ENV["JULIA_STACKTRACE_MINIMAL"] = true #jl
-using Revise #jl
-import Rible as RB
-figdir::String = joinpath(pathof(RB),"../../tmp")
-if Sys.iswindows() #src
-    figdir::String = raw"C:\Users\luo22\OneDrive\Papers\FrictionalContact\CMAME" #src
-elseif Sys.isapple() #src
-    figdir::String = raw"/Users/jacob/Library/CloudStorage/OneDrive-SharedLibraries-onedrive/Papers/FrictionalContact/CMAME" #src
-end #src
-include("../../vis.jl")
-includet("../../vis.jl") #jl
+
+figdir::String = ""
+if Sys.iswindows()
+    figdir::String = raw"C:\Users\luo22\OneDrive\Papers\TensegrityStability"
+elseif Sys.isapple()
+    figdir::String = raw"/Users/jacob/Library/CloudStorage/OneDrive-SharedLibraries-onedrive/Papers/TensegrityStability"
+end
+
+include(joinpath(pathof(RB),"../../test/vis.jl"))
+includet(joinpath(pathof(RB),"../../test/vis.jl")) #jl
 
 #-- T bars
-include("../../../examples/bodies/make_3d_bar.jl")
-includet("../../../examples/bodies/make_3d_bar.jl") #jl
-include("../../../examples/robots/Tbars.jl")
-includet("../../../examples/robots/Tbars.jl")#jl
+include(joinpath(pathof(RB),"../../examples/bodies/make_3d_bar.jl"))
+includet(joinpath(pathof(RB),"../../examples/bodies/make_3d_bar.jl")) #jl
+include(joinpath(pathof(RB),"../../examples/robots/Tbars.jl"))
+includet(joinpath(pathof(RB),"../../examples/robots/Tbars.jl"))#jl
+
 tbbot = Tbars(;θ=π/4)
 bot = tbbot
 @myshow bot.structure.connectivity.indexed.num_of_dof
@@ -39,8 +44,8 @@ plot_traj!(bot;showarrows = false, showground=false)
 
 #todo slider with no hook
 GM.activate!();with_theme(theme_pub;
-        size = (1tw,0.15tw),
-        figure_padding = (0,0,fontsize/2,0),
+        size = (1cw,0.6cw),
+        figure_padding = (0,0,-fontsize/2,0),
         Axis3 = (
             # azimuth = -1.8701322643948965,
             # elevation = 0.6128666392948965,
@@ -51,9 +56,10 @@ GM.activate!();with_theme(theme_pub;
     ) do 
     fig = Figure()
     gd0 = fig[1,1] = GridLayout(;tellheight=false)
-    gd1 = fig[1,2] = GridLayout(;tellheight=false)
+    gd1 = fig[2,1] = GridLayout(;tellheight=false)
     # rowsize!(gd0,1,Fixed(0.15tw))
     # rowsize!(gd1,1,Fixed(0.15tw))
+    rowgap!(fig.layout,0)
     bot0 = Tbars(;θ=0)
     plot_traj!(
         bot0,
@@ -75,7 +81,7 @@ GM.activate!();with_theme(theme_pub;
         end,
         sup! = (ax,tgob,sgi)-> begin
             # cables
-            hidez(ax)
+            RB.hidez(ax)
         end
     )
     bot1 = Tbars(;θ=π/4)
@@ -98,7 +104,7 @@ GM.activate!();with_theme(theme_pub;
         end,
         sup! = (ax,tgob,sgi)-> begin
             # cables
-            hidez(ax)
+            RB.hidez(ax)
         end
     )
     savefig(fig,"Tbars")
@@ -188,7 +194,7 @@ struct𝒦 = [
         # @show s
         λ = inv(Ǎ*transpose(Ǎ))*Ǎ*Bᵀ*s
         @show λ
-        Ǩa = - RB.cstr_forces_jacobian(bot.structure,q,λ)
+        Ǩa = RB.cstr_forces_jacobian(bot.structure,q,λ)
 
         Ǩg = RB.build_geometric_stiffness_matrix!(bot.structure,q,s)
 
@@ -208,8 +214,9 @@ mat𝒦as = reduce(hcat,struct𝒦.𝒦a)
 mat𝒦ps = reduce(hcat,struct𝒦.𝒦p)
 
 GM.activate!();with_theme(theme_pub;
-        size = (0.95tw,0.18tw),
-        figure_padding = (0,0,0,0),
+        size = (1cw,0.72cw),
+        fontsize=6.5,
+        figure_padding = (-fontsize,-fontsize,-2fontsize,0),
         Axis3 = (
             azimuth = -π/2-1e-10,
             elevation = π/2,
@@ -226,7 +233,7 @@ GM.activate!();with_theme(theme_pub;
         bot0,
         fig = gd,
         AxisType=Axis3,
-        gridsize = (1,4),
+        gridsize = (2,2),
         showpoints = false,
         showlabels = false,
         showground = false,
@@ -234,7 +241,7 @@ GM.activate!();with_theme(theme_pub;
         showcables = false,
         xlims = (-2.2,1.2),
         ylims = (-1.2,1.2),
-        rowgap=0,
+        rowgap=-fontsize,
         titleformatfunc = (sgi,tt)-> begin
             rich(
                     rich("($(alphabet[sgi])) ", font=:bold),
@@ -251,7 +258,7 @@ GM.activate!();with_theme(theme_pub;
             RB.hidexyz(ax)
             @myshow Sbool[:,sgi]
             linesegs_cables = @lift begin
-                get_linesegs_cables($tgob;)[Sbool[:,sgi]]
+                RB.get_linesegs_cables($tgob;)[Sbool[:,sgi]]
             end
             linesegments!(ax, 
                 linesegs_cables, 
@@ -259,18 +266,18 @@ GM.activate!();with_theme(theme_pub;
                 # linewidth = cablewidth
                 )
             rcs_by_cables = @lift begin
-                (;tensioned) = $tgob.connectivity
+                cables = RB.get_cables($tgob)
                 num_of_dim = RB.get_num_of_dims($tgob)
                 T = RB.get_numbertype($tgob)
                 ret = Vector{MVector{num_of_dim,T}}()
                 mapreduce(
-                    (scnt)->
+                    (cable)->
                     [(
-                        scnt.hen.body.state.loci_states[scnt.hen.pid].position.+
-                        scnt.egg.body.state.loci_states[scnt.egg.pid].position
+                        cable.joint.hen2egg.hen.body.state.loci_states[cable.joint.hen2egg.hen.pid].frame.position.+
+                        cable.joint.hen2egg.egg.body.state.loci_states[cable.joint.hen2egg.egg.pid].frame.position
                     )./2],
                     vcat,
-                    tensioned.connected
+                    cables
                     ;init=ret
                 )
             end
@@ -285,14 +292,14 @@ GM.activate!();with_theme(theme_pub;
             #     ax,
             #     rcs_by_cables[][Sbool[:,sgi]],
             #     marker = :rect, 
-            #     markersize = 12 |> pt2px, 
+            #     markersize = 12 , 
             #     color = :white
             # )
             text!(
                 ax,
                 Stext,
                 position = rcs_by_cables[][Sbool[:,sgi]],
-                fontsize = 5 |> pt2px,
+                fontsize = 5 ,
                 color = :red,
                 align = (:left, :top),
                 offset = (fontsize/4, 0)
@@ -327,7 +334,8 @@ Vals =  [
 ] |> VectorOfArray
 
 GM.activate!();with_theme(theme_pub;
-        size = (0.45tw,0.2tw),
+        size = (0.8cw,0.3cw),
+        fontsize = 6.5,
         figure_padding = (0,fontsize,0,fontsize),
     ) do 
     fig = Figure()
diff --git a/yard/stiffness/main.jl b/yard/stiffness/main.jl
index db7b9d0..e2f98dc 100644
--- a/yard/stiffness/main.jl
+++ b/yard/stiffness/main.jl
@@ -1,3 +1,12 @@
+
+using Revise #jl
+import Rible as RB
+
+include(joinpath(pathof(RB),"../../yard/stability_stiffness.jl"))
+using AbbreviatedStackTraces #jl
+ENV["JULIA_STACKTRACE_ABBREVIATED"] = true #jl
+ENV["JULIA_STACKTRACE_MINIMAL"] = true #jl
+
 figdir::String = ""
 if Sys.iswindows()
     figdir::String = raw"C:\Users\luo22\OneDrive\Papers\TensegrityStability"
@@ -5,14 +14,35 @@ elseif Sys.isapple()
     figdir::String = raw"/Users/jacob/Library/CloudStorage/OneDrive-SharedLibraries-onedrive/Papers/TensegrityStability"
 end
 
-includet("deps.jl")
-import Rible as RB
-include("../../vis.jl")
-includet("../../vis.jl")
+include(joinpath(pathof(RB),"../../test/vis.jl"))
+includet(joinpath(pathof(RB),"../../test/vis.jl")) #jl
+
+include(joinpath(pathof(RB),"../../examples/bodies/rigidbar.jl"))
+includet(joinpath(pathof(RB),"../../examples/bodies/rigidbar.jl")) #jl
+include(joinpath(pathof(RB),"../../examples/robots/superball.jl"))
+includet(joinpath(pathof(RB),"../../examples/robots/superball.jl")) #jl
 
-fontsize = 8 |> pt2px
-tw = 468 |> pt2px
-th = 622 |> pt2px
+
+include(joinpath(pathof(RB),"../../examples/bodies/make_3d_bar.jl"))
+includet(joinpath(pathof(RB),"../../examples/bodies/make_3d_bar.jl")) #jl
+include(joinpath(pathof(RB),"../../examples/bodies/make_3d_plate.jl"))
+includet(joinpath(pathof(RB),"../../examples/bodies/make_3d_plate.jl")) #jl
+include(joinpath(pathof(RB),"../../examples/robots/prism.jl"))
+includet(joinpath(pathof(RB),"../../examples/robots/prism.jl")) #jl
+
+include(joinpath(pathof(RB),"../../examples/bodies/make_3d_tri.jl"))
+includet(joinpath(pathof(RB),"../../examples/bodies/make_3d_tri.jl")) #jl
+include(joinpath(pathof(RB),"../../examples/robots/tower.jl"))
+includet(joinpath(pathof(RB),"../../examples/robots/tower.jl")) #jl
+
+include(joinpath(pathof(RB),"../../examples/bodies/build_2d_tri.jl"))
+includet(joinpath(pathof(RB),"../../examples/bodies/build_2d_tri.jl")) #jl
+include(joinpath(pathof(RB),"../../examples/robots/two_tri.jl"))
+includet(joinpath(pathof(RB),"../../examples/robots/two_tri.jl")) #jl
+
+fontsize = 8 
+cw = 240.7103
+tw = 494.22432
 #-- preamble end
 
 #-- one_tri_one_bar
@@ -52,7 +82,7 @@ end
 function build_nullspace_on_free(st)
     (;sys_free_coords_idx,bodyid2sys_dof_idx) = st.connectivity.indexed
     q = RB.get_coords(bot.structure)
-    Nin = RB.make_intrinsic_nullspace(st,q)[
+    Nin = RB.intrinsic_nullspace(st,q)[
         sys_free_coords_idx,
         reduce(vcat,bodyid2sys_dof_idx[2:end])
     ]
@@ -75,13 +105,13 @@ f = RB.get_cables_tension(bot)
 
 function verify_lambda(st)
     T = RB.get_numbertype(st)
-    λs = zeros(T,st.num_of_bodies)
+    λs = zeros(T,st.connectivity.indexed.num_of_bodies)
     foreach(st.bodies) do body
         (;prop,state) = body
-        (;loci_states,origin_position) = state
+        (;loci_states,origin_frame) = state
         # @myshow prop.id
         for locus_state in loci_states
-            λs[prop.id] += -1/2*(locus_state.position-origin_position)'*locus_state.force
+            λs[prop.id] += -1/2*(locus_state.frame.position-origin_frame.position)'*locus_state.force
         end
     end
     λs
@@ -118,7 +148,7 @@ l = RB.get_cables_len(bot.structure)
 λ = inv(Ǎ*transpose(Ǎ))*Ǎ*Bᵀ*f
 @myshow verify_lambda(bot.structure)
 @myshow λ
-Ǩa = RB.cstr_forces_jacobian(bot.structure,λ)
+Ǩa = RB.cstr_forces_jacobian(bot.structure,q,λ)
 𝒦a = transpose(Ň)*Ǩa*Ň |> Symmetric 
 vals_𝒦a,vecs_𝒦a = eigen(𝒦a)
 @myshow sort(vals_𝒦a)
@@ -136,7 +166,7 @@ vec𝒦ps = [
         # @show s
         λi = inv(Ǎ*transpose(Ǎ))*Ǎ*Bᵀ*si
         # @show f,λ
-        Ǩai = - RB.cstr_forces_jacobian(bot.structure,λi)
+        Ǩai = RB.cstr_forces_jacobian(bot.structure,q,λi)
 
         Ǩgi = RB.build_geometric_stiffness_matrix!(bot.structure,q,si)
 
@@ -171,10 +201,11 @@ v'*𝒦*v
 vm[:,1] = v
 orthovm = RB.modified_gram_schmidt(vm)
 
-with_theme(theme_pub;
-    size = (0.9tw,0.3tw),
-    fontsize = 6.5 |> pt2px,
-    figure_padding = (2fontsize,fontsize,0,0),
+
+GM.activate!(;scalefactor=2);with_theme(theme_pub;
+    size = (1cw,0.9cw),
+    fontsize = 6.5 ,
+    figure_padding = (0,0,-2.5fontsize,0),
     Axis3 = (        
         azimuth = 3.7555306333269844,
         elevation = 0.3726990816987242,
@@ -190,12 +221,16 @@ with_theme(theme_pub;
         ratio = norm(δq̌i)/norm(q̌)
         botvis.traj.q̌[end] .= q̌ .+ scaling.*δq̌i/ratio
     end
+    fig = Figure()
+    gd1 = fig[1,1:2] = GridLayout()
+    gd2 = fig[2,1:2] = GridLayout()
+    rowgap!(fig.layout,0)
     plot_traj!(
         botvis;
-        figsize = (0.8tw,0.26tw),
+        fig = gd1,
         AxisType=Axis3,
-        gridsize=(1,nk+1),        
-        atsteps=1:nk+1,
+        gridsize=(1,1),
+        atsteps=1:1,
         doslide=false,
         showlabels=false,
         showpoints=false,
@@ -210,20 +245,42 @@ with_theme(theme_pub;
                     rich("($(alphabet[sgi])) ", font=:bold),
                     [
                         "Initial",
+                    ][sgi]
+                )
+        end,
+    )
+    plot_traj!(
+        botvis;
+        fig = gd2,
+        AxisType=Axis3,
+        gridsize=(1,nk),        
+        atsteps=2:nk+1,
+        doslide=false,
+        showlabels=false,
+        showpoints=false,
+        # showcables = false,
+        showground = false,
+        xlims = (-1e0,1e0),
+        ylims = (-1e0,1e0),
+        zlims = (-1e-5,2e0),
+        slack_linestyle = :solid,
+        showinit = true,titleformatfunc = (sgi,tt)-> begin
+            rich(
+                    rich("($(alphabet[sgi+1])) ", font=:bold),
+                    [
                         "Mechanism Mode 1",
                         "Mechanism Mode 2"
                     ][sgi]
                 )
         end,
         sup! = (ax,tgob,sgi)->begin
-            if sgi != 1
-                hidedecorations!(ax)
-                xlims!(ax,-1.0e0,1.2e0)
-                ylims!(ax,-1.2e0,1.0e0)
-            end
+            hidedecorations!(ax)
+            xlims!(ax,-1.0e0,1.2e0)
+            ylims!(ax,-1.2e0,1.0e0)
         end,
-        figname="superball"
     )
+    savefig(fig,"superball")
+    fig
 end
 
 Ňv = Ň*nullspace(v')
@@ -254,7 +311,7 @@ vecr𝒦ps = [
         # @show s
         λi = inv(Ǎ*transpose(Ǎ))*Ǎ*Bᵀ*si
         # @show f,λ
-        Ǩai = RB.cstr_forces_jacobian(bot.structure,λi)
+        Ǩai = RB.cstr_forces_jacobian(bot.structure,q,λi)
 
         Ǩgi = RB.build_geometric_stiffness_matrix!(bot.structure,q,si)
 
@@ -305,7 +362,7 @@ maxminmodes = hcat(
 )
 
 with_theme(theme_pub;
-    fontsize = 6.5 |> pt2px,
+    fontsize = 6.5 ,
     size = (0.8tw,0.18tw),
     figure_padding = (0,0,-fontsize,0),
     Axis3 = (        
@@ -378,7 +435,7 @@ size(rρs,1)
 ]
 
 with_theme(theme_pub;
-        size = (0.3tw,0.2tw),
+        size = (0.6cw,0.35cw),
         figure_padding = (0,fontsize,0,fontsize),
     ) do 
     fig = Figure()
@@ -388,7 +445,7 @@ with_theme(theme_pub;
     )
     lines!(ax,σs,rρs[1,:],)
     xlims!(ax,0,5500)
-    ylims!(ax,-400,600)
+    ylims!(ax,-200,600)
     # for i = axes(rρs,1)
     #     lines!(ax,σs,rρs[i,:],)
     # end
@@ -427,18 +484,17 @@ prism1 = prisms(;
 )
 bot = prism1
 rb1 = RB.get_bodies(bot)[1]
-viz(rb1) 
+ 
 plot_traj!(bot;showground=false)
 RB.check_static_equilibrium_output_multipliers(bot.structure)
-@myshow bot.structure.num_of_dof
+@myshow bot.structure.connectivity.indexed.num_of_dof
 RB.update!(bot.structure)
 f = RB.get_cables_tension(bot)
-
 # for use with Class-1 and the 1st rigid fixed
 function build_nullspace_on_free(st)
     (;sys_free_coords_idx,bodyid2sys_full_coords,bodyid2sys_dof_idx) = st.connectivity.indexed
     q = RB.get_coords(bot.structure)
-    Nin = RB.make_intrinsic_nullspace(st,q)[
+    Nin = RB.intrinsic_nullspace(st,q)[
         sys_free_coords_idx,
         reduce(vcat,bodyid2sys_dof_idx[begin:end-1])
     ]
@@ -487,10 +543,10 @@ D
 ns = size(S,2)
 nk = size(D,2)
 
-GM.activate!();with_theme(theme_pub;
-        size = (0.95tw,0.24tw),
-        figure_padding = (2fontsize,0,0,0),
-        fontsize = 6.5 |> pt2px,
+GM.activate!(;scalefactor=4);with_theme(theme_pub;
+        size = (1cw,0.30tw),
+        figure_padding = (0,0,-2fontsize,0),
+        fontsize = 6.5 ,
         Axis3 = (
             azimuth = 3.8255306333269843,
             elevation = 0.2026990816987241
@@ -503,7 +559,10 @@ GM.activate!();with_theme(theme_pub;
     fig = Figure()
     gd1 = fig[1,1] = GridLayout()
     gd2 = fig[1,2:3] = GridLayout()
-    gd3 = fig[1,4:5] = GridLayout()
+    gd3_holder = fig[2,1:3] = GridLayout()
+    gd3 = gd3_holder[1,1:2] = GridLayout()
+    colgap!(fig.layout,0)
+    rowgap!(fig.layout,-fontsize)
     botmm = deepcopy(bot)
     plot_traj!(
         bot;
@@ -524,8 +583,8 @@ GM.activate!();with_theme(theme_pub;
                 )
         end,
         sup! = (ax,tgob,sgi)->begin
-            hidex(ax)
-            hidey(ax)
+            RB.hidex(ax)
+            RB.hidey(ax)
             # xlims!(ax,-1.0e0,1.2e0)
             # ylims!(ax,-1.2e0,1.0e0)
         end,
@@ -560,10 +619,10 @@ GM.activate!();with_theme(theme_pub;
             ax.elevation = 0.18269908169872395
             # azimuth = 4.665530633326984
             # elevation = 0.16269908169872424
-            hidexyz(ax)
+            RB.hidexyz(ax)
             @myshow Sbool[:,sgi]
             linesegs_cables = @lift begin
-                get_linesegs_cables($tgob;)[Sbool[:,sgi]]
+                RB.get_linesegs_cables($tgob;)[Sbool[:,sgi]]
             end
             linesegments!(ax, 
                 linesegs_cables, 
@@ -571,18 +630,18 @@ GM.activate!();with_theme(theme_pub;
                 # linewidth = cablewidth
                 )
             rcs_by_cables = @lift begin
-                (;tensioned) = $tgob.connectivity
+                cables = RB.get_cables($tgob)
                 num_of_dim = RB.get_num_of_dims($tgob)
                 T = RB.get_numbertype($tgob)
                 ret = Vector{MVector{num_of_dim,T}}()
                 mapreduce(
-                    (scnt)->
+                    (cable)->
                     [(
-                        scnt.hen.body.state.loci_states[scnt.hen.pid].position.+
-                        scnt.egg.body.state.loci_states[scnt.egg.pid].position
+                        cable.joint.hen2egg.hen.body.state.loci_states[cable.joint.hen2egg.hen.pid].frame.position.+
+                        cable.joint.hen2egg.egg.body.state.loci_states[cable.joint.hen2egg.egg.pid].frame.position
                     )./2],
                     vcat,
-                    tensioned.connected
+                    cables
                     ;init=ret
                 )
             end
@@ -597,14 +656,14 @@ GM.activate!();with_theme(theme_pub;
             #     ax,
             #     rcs_by_cables[][Sbool[:,sgi]],
             #     marker = :rect, 
-            #     markersize = 12 |> pt2px, 
+            #     markersize = 12 , 
             #     color = :white
             # )
             text!(
                 ax,
                 Stext,
                 position = rcs_by_cables[][Sbool[:,sgi]],
-                fontsize = 5 |> pt2px,
+                fontsize = 5 ,
                 color = :red,
                 align = (:center, :center),
                 # offset = (-fontsize/2, 0)
@@ -633,7 +692,7 @@ GM.activate!();with_theme(theme_pub;
         showground = false,
         xlims = (-8e-2,8e-2),
         ylims = (-8e-2,8e-2),
-        zlims = (-1e-5,2.5e-1),
+        zlims = (-1e-5,2.1e-1),
         slack_linestyle = :solid,
         showinit = true,titleformatfunc = (sgi,tt)-> begin
             rich(
@@ -645,7 +704,7 @@ GM.activate!();with_theme(theme_pub;
                 )
         end,
         sup! = (ax,tgob,sgi)->begin
-            hidexyz(ax)
+            RB.hidexyz(ax)
             # xlims!(ax,-1.0e0,1.2e0)
             # ylims!(ax,-1.2e0,1.0e0)
         end,
@@ -670,7 +729,7 @@ f = S*ᾱ
 
 λ = inv(Ǎ*transpose(Ǎ))*Ǎ*Bᵀ*f
 # @show f,λ
-Ǩa = RB.cstr_forces_jacobian(bot.structure,λ)
+Ǩa = RB.cstr_forces_jacobian(bot.structure,q,λ)
 𝒦ain = transpose(Nin)*Ǩa*Nin
 𝒦a = transpose(Ň)*Ǩa*Ň |> Symmetric 
 vals_𝒦a,vecs_𝒦a = eigen(𝒦a)
@@ -697,7 +756,7 @@ vec𝒦ps = [
         # @show s
         λi = inv(Ǎ*transpose(Ǎ))*Ǎ*Bᵀ*si
         # @show f,λ
-        Ǩai = RB.cstr_forces_jacobian(bot.structure,λi)
+        Ǩai = RB.cstr_forces_jacobian(bot.structure,q,λi)
 
         Ǩgi = RB.build_geometric_stiffness_matrix!(bot.structure,q,si)
 
@@ -760,7 +819,8 @@ Vals =  [
 ] |> VectorOfArray
 
 with_theme(theme_pub;
-        size = (0.6tw,0.2tw),
+        size = (1cw,0.3cw),
+        fontsize = 6.5,
         figure_padding = (0,fontsize,0,0),
     ) do 
     fig = Figure()
@@ -865,7 +925,8 @@ Vals_α =  [
 ] |> VectorOfArray
 
 with_theme(theme_pub;
-        size = (0.6tw,0.2tw),
+        size = (1cw,0.3cw),
+        fontsize = 6.5,
         figure_padding = (0,fontsize,0,0),
     ) do 
     fig = Figure()
@@ -925,14 +986,14 @@ end
 two = two_tri()
 bot = two
 plot_traj!(bot;showground=false)
-bot.structure.num_of_dof
+bot.structure.connectivity.indexed.num_of_dof
 
 RB.check_static_equilibrium_output_multipliers(bot.structure)
 
 function make_nullspace_on_free(st)    
     (;sys_free_coords_idx,bodyid2sys_dof_idx) = st.connectivity.indexed
     q = RB.get_coords(bot.structure)
-    Nin = RB.make_intrinsic_nullspace(st,q)
+    Nin = RB.intrinsic_nullspace(st,q)
     Nin[
         sys_free_coords_idx,
         reduce(vcat,bodyid2sys_dof_idx[2:end])
@@ -985,9 +1046,9 @@ nk = size(D,2)
 ns = size(S,2)
 
 GM.activate!();with_theme(theme_pub;
-        size = (0.9tw,0.22tw),
-        figure_padding = (fontsize,0,0,0),
-        fontsize = 6.5 |> pt2px,
+        size = (1cw,0.6cw),
+        figure_padding = (-1fontsize,-1fontsize,-1fontsize,0),
+        fontsize = 6.5 ,
         Axis3 = (
             azimuth = -π/2-1e-10,
             elevation = π/2,
@@ -1001,8 +1062,11 @@ GM.activate!();with_theme(theme_pub;
     Sbool = S.> maxS*rtol
     S[.!Sbool] .= 0.0
     fig = Figure()
-    gd1 = fig[1,1] = GridLayout(;tellheight=false)
-    gd2 = fig[1,2:3] = GridLayout(;tellheight=false)
+    gd1 = fig[1,1:2] = GridLayout(;tellheight=false)
+    gd2 = fig[2,1:2] = GridLayout(;tellheight=false)
+    ## colgap!(gd2,-fontsize)
+    rowsize!(fig.layout,1,Relative(0.3))
+    rowgap!(fig.layout,0fontsize)
     plot_traj!(
         bot,
         fig = gd1,
@@ -1021,10 +1085,10 @@ GM.activate!();with_theme(theme_pub;
         end,
         sup! = (ax,tgob,sgi)-> begin
             # cables
-            hidez(ax)
+            RB.hidez(ax)
         end
     )
-    rowsize!(gd1,1,Fixed(0.1tw))
+    ## rowsize!(gd1,1,Fixed(0.3cw))
     plot_traj!(
         bot,
         fig = gd2,
@@ -1037,7 +1101,8 @@ GM.activate!();with_theme(theme_pub;
         showcables = false,
         xlims = (-0.5,0.5),
         ylims = (-0.15,0.15),
-        rowgap=0,
+        rowgap=-1fontsize,
+        colgap=-2fontsize,
         titleformatfunc = (sgi,tt)-> begin
             rich(
                     rich("($(alphabet[sgi+1])) ", font=:bold),
@@ -1051,10 +1116,10 @@ GM.activate!();with_theme(theme_pub;
         end,
         sup! = (ax,tgob,sgi)-> begin
             # cables
-            hidexyz(ax)
+            RB.hidexyz(ax)
             @myshow Sbool[:,sgi]
             linesegs_cables = @lift begin
-                get_linesegs_cables($tgob;)[Sbool[:,sgi]]
+                RB.get_linesegs_cables($tgob;)[Sbool[:,sgi]]
             end
             linesegments!(ax, 
                 linesegs_cables, 
@@ -1062,18 +1127,18 @@ GM.activate!();with_theme(theme_pub;
                 # linewidth = cablewidth
                 )
             rcs_by_cables = @lift begin
-                (;tensioned) = $tgob.connectivity
+                cables = RB.get_cables($tgob)
                 num_of_dim = RB.get_num_of_dims($tgob)
                 T = RB.get_numbertype($tgob)
                 ret = Vector{MVector{num_of_dim,T}}()
                 mapreduce(
-                    (scnt)->
+                    (cable)->
                     [(
-                        scnt.hen.body.state.loci_states[scnt.hen.pid].position.+
-                        scnt.egg.body.state.loci_states[scnt.egg.pid].position
+                        cable.joint.hen2egg.hen.body.state.loci_states[cable.joint.hen2egg.hen.pid].frame.position.+
+                        cable.joint.hen2egg.egg.body.state.loci_states[cable.joint.hen2egg.egg.pid].frame.position
                     )./2],
                     vcat,
-                    tensioned.connected
+                    cables
                     ;init=ret
                 )
             end
@@ -1088,14 +1153,14 @@ GM.activate!();with_theme(theme_pub;
             #     ax,
             #     rcs_by_cables[][Sbool[:,sgi]],
             #     marker = :rect, 
-            #     markersize = 12 |> pt2px, 
+            #     markersize = 12 , 
             #     color = :white
             # )
             text!(
                 ax,
                 Stext,
                 position = rcs_by_cables[][Sbool[:,sgi]],
-                fontsize = 5 |> pt2px,
+                fontsize = 5 ,
                 color = :red,
                 align = (:center, :center),
                 # offset = (-fontsize/2, 0)
@@ -1121,7 +1186,7 @@ struct𝒦 = [
         # @show s
         λ = inv(Ǎ*transpose(Ǎ))*Ǎ*Bᵀ*s
         # @show f,λ
-        Ǩa = RB.cstr_forces_jacobian(bot.structure,λ)
+        Ǩa = RB.cstr_forces_jacobian(bot.structure,q,λ)
 
         Ǩg = RB.build_geometric_stiffness_matrix!(bot.structure,q,s)
 
@@ -1159,7 +1224,7 @@ Vals =  [
 ] |> VectorOfArray
 
 GM.activate!();with_theme(theme_pub;
-        size = (0.5tw,0.2tw),
+        size = (0.8cw,0.3cw),
         figure_padding = (0,fontsize,0,fontsize),
     ) do 
     fig = Figure()
@@ -1235,7 +1300,7 @@ f = sum(S,dims=2)
 # equivalent μ
 # μ = l .- (f./k)
 
-λ = -inv(Ǎ*transpose(Ǎ))*Ǎ*Bᵀ*f
+λ = inv(Ǎ*transpose(Ǎ))*Ǎ*Bᵀ*f
 # @show f,λ
 Ǩa = RB.cstr_forces_jacobian(bot.structure,λ)
 𝒦a = transpose(Ň)*Ǩa*Ň |> Symmetric 
@@ -1255,7 +1320,7 @@ vec𝒦ps = [
         # @show s
         λi = inv(Ǎ*transpose(Ǎ))*Ǎ*Bᵀ*si
         # @show f,λ
-        Ǩai = - RB.cstr_forces_jacobian(bot.structure,λi)
+        Ǩai = RB.cstr_forces_jacobian(bot.structure,λi)
 
         Ǩgi = RB.build_geometric_stiffness_matrix!(bot.structure,q,si)
 
@@ -1380,14 +1445,14 @@ end
 towerbot = tower()
 bot = towerbot
 
-@myshow bot.structure.num_of_dof
+@myshow bot.structure.connectivity.indexed.num_of_dof
 
 plot_traj!(bot;showground=false)
 
 function build_nullspace_on_free(st)
     (;sys_free_coords_idx,bodyid2sys_full_coords,bodyid2sys_dof_idx) = st.connectivity.indexed
     q = RB.get_coords(bot.structure)
-    Nin = RB.make_intrinsic_nullspace(st,q)[
+    Nin = RB.intrinsic_nullspace(st,q)[
         sys_free_coords_idx,
         reduce(vcat,bodyid2sys_dof_idx[begin+1:end])
     ]    
@@ -1416,7 +1481,7 @@ Ǎ = RB.make_cstr_jacobian(bot.structure)(q)
 Ň = build_nullspace_on_free(bot.structure)
 Q̃ = RB.build_Q̃(bot.structure)
 L̂ = RB.build_L̂(bot.structure)
-
+rank(Ň)
 Ǎ*Ň |> norm
 
 # Left hand side
@@ -1436,9 +1501,9 @@ l = RB.get_cables_len(bot.structure)
 
 isis = [8,14,24]
 GM.activate!();with_theme(theme_pub;
-        size = (0.95tw,0.2tw),
-        figure_padding = (2fontsize,0,0,0),
-        fontsize = 6.5 |> pt2px,
+        size = (1cw,0.3tw),
+        figure_padding = (-fontsize,-fontsize,-2fontsize,0),
+        fontsize = 6.5 ,
         Axis3 = (
             azimuth = 3.8255306333269843,
             elevation = 0.2026990816987241
@@ -1450,7 +1515,10 @@ GM.activate!();with_theme(theme_pub;
     S[.!Sbool] .= 0.0
     fig = Figure()
     gd1 = fig[1,1] = GridLayout()
-    gd2 = fig[1,2:3+1] = GridLayout()
+    gd2_holder = fig[2,:] = GridLayout()
+    gd2 = gd2_holder[1,1:3] = GridLayout()
+    ## rowsize!(fig.layout,1,Relative(0.3))
+    rowgap!(fig.layout,-1fontsize)
     botmm = deepcopy(bot)
     plot_traj!(
         bot;
@@ -1471,8 +1539,8 @@ GM.activate!();with_theme(theme_pub;
                 )
         end,
         sup! = (ax,tgob,sgi)->begin
-            hidex(ax)
-            hidey(ax)
+            RB.hidex(ax)
+            RB.hidey(ax)
             # xlims!(ax,-1.0e0,1.2e0)
             # ylims!(ax,-1.2e0,1.0e0)
         end,
@@ -1489,12 +1557,13 @@ GM.activate!();with_theme(theme_pub;
         xlims = (-1.5e0,1.5e0),
         ylims = (-1.5e0,1.5e0),
         zlims = (-0.6e0,1.5e0),
+        colgap = -2fontsize,
         showground = false,
         showinit = true,
         titleformatfunc = (sgi,tt)-> begin
             rich(
                     rich("($(alphabet[sgi+1])) ", font=:bold),
-                    "Self-stress State $sgi"
+                    "Self-stress State $(isis[sgi])"
                 )
         end,
         sup! = (ax,tgob,sgi_input)-> begin
@@ -1504,10 +1573,10 @@ GM.activate!();with_theme(theme_pub;
             ax.elevation = 0.18269908169872395
             # azimuth = 4.665530633326984
             # elevation = 0.16269908169872424
-            hidexyz(ax)
+            RB.hidexyz(ax)
             @myshow Sbool[:,sgi]
             linesegs_cables = @lift begin
-                get_linesegs_cables($tgob;)[Sbool[:,sgi]]
+                RB.get_linesegs_cables($tgob;)[Sbool[:,sgi]]
             end
             linesegments!(ax, 
                 linesegs_cables, 
@@ -1515,18 +1584,18 @@ GM.activate!();with_theme(theme_pub;
                 # linewidth = cablewidth
                 )
             rcs_by_cables = @lift begin
-                (;tensioned) = $tgob.connectivity
+                cables = RB.get_cables($tgob)
                 num_of_dim = RB.get_num_of_dims($tgob)
                 T = RB.get_numbertype($tgob)
                 ret = Vector{MVector{num_of_dim,T}}()
                 mapreduce(
-                    (scnt)->
+                    (cable)->
                     [(
-                        scnt.hen.body.state.loci_states[scnt.hen.pid].+
-                        scnt.egg.body.state.loci_states[scnt.egg.pid]
+                        cable.joint.hen2egg.hen.body.state.loci_states[cable.joint.hen2egg.hen.pid].frame.position.+
+                        cable.joint.hen2egg.egg.body.state.loci_states[cable.joint.hen2egg.egg.pid].frame.position
                     )./2],
                     vcat,
-                    tensioned.connected
+                    cables
                     ;init=ret
                 )
             end
@@ -1541,14 +1610,14 @@ GM.activate!();with_theme(theme_pub;
             #     ax,
             #     rcs_by_cables[][Sbool[:,sgi]],
             #     marker = :rect, 
-            #     markersize = 12 |> pt2px, 
+            #     markersize = 12 , 
             #     color = :white
             # )
             text!(
                 ax,
                 Stext,
                 position = rcs_by_cables[][Sbool[:,sgi]],
-                fontsize = 5 |> pt2px,
+                fontsize = 5 ,
                 color = :red,
                 align = (:center, :center),
                 # offset = (-fontsize/2, 0)
@@ -1566,9 +1635,9 @@ f = S*ᾱ
 # equivalent μ
 # μ = l .- (f./k)
 
-λ = -inv(Ǎ*transpose(Ǎ))*Ǎ*Bᵀ*f
+λ = inv(Ǎ*transpose(Ǎ))*Ǎ*Bᵀ*f
 # @show f,λ
-Ǩa = RB.cstr_forces_jacobian(bot.structure,λ)
+Ǩa = RB.cstr_forces_jacobian(bot.structure,q,λ)
 𝒦a = transpose(Ň)*Ǩa*Ň |> Symmetric 
 vals_𝒦a,vecs_𝒦a = eigen(𝒦a)
 @myshow sort(vals_𝒦a)
@@ -1588,7 +1657,7 @@ vec𝒦ps = [
         # @show s
         λi = inv(Ǎ*transpose(Ǎ))*Ǎ*Bᵀ*si
         # @show f,λ
-        Ǩai = - RB.cstr_forces_jacobian(bot.structure,λi)
+        Ǩai = RB.cstr_forces_jacobian(bot.structure,q,λi)
 
         Ǩgi = RB.build_geometric_stiffness_matrix!(bot.structure,q,si)
 
@@ -1661,7 +1730,8 @@ Vals_alpha3 =  [
 
 
 with_theme(theme_pub;
-        size = (0.35tw,0.2tw),
+        size = (0.6cw,0.36cw),
+        fontsize = 6.5,
         figure_padding = (0,fontsize,0,fontsize),
     ) do 
     fig = Figure()
@@ -1779,7 +1849,7 @@ f =  S*ᾱ
 GM.activate!();with_theme(theme_pub;
         size = (0.95tw,0.24tw),
         figure_padding = (2fontsize,0,0,0),
-        fontsize = 6.5 |> pt2px,
+        fontsize = 6.5 ,
         Axis3 = (
             azimuth = 3.8255306333269843,
             elevation = 0.2026990816987241
@@ -1844,7 +1914,7 @@ GM.activate!();with_theme(theme_pub;
             ax.elevation = 0.18269908169872395
             # azimuth = 4.665530633326984
             # elevation = 0.16269908169872424
-            hidexyz(ax)
+            RB.hidexyz(ax)
             @myshow Sbool[:,sgi]
             linesegs_cables = @lift begin
                 get_linesegs_cables($tgob;)[Sbool[:,sgi]]
@@ -1881,14 +1951,14 @@ GM.activate!();with_theme(theme_pub;
             #     ax,
             #     rcs_by_cables[][Sbool[:,sgi]],
             #     marker = :rect, 
-            #     markersize = 12 |> pt2px, 
+            #     markersize = 12 , 
             #     color = :white
             # )
             text!(
                 ax,
                 Stext,
                 position = rcs_by_cables[][Sbool[:,sgi]],
-                fontsize = 5 |> pt2px,
+                fontsize = 5 ,
                 color = :red,
                 align = (:center, :center),
                 # offset = (-fontsize/2, 0)
@@ -1899,7 +1969,7 @@ GM.activate!();with_theme(theme_pub;
     fig
 end
 
-λ = -inv(Ǎ*transpose(Ǎ))*Ǎ*Bᵀ*f
+λ = inv(Ǎ*transpose(Ǎ))*Ǎ*Bᵀ*f
 # @show f,λ
 Ǩa = RB.cstr_forces_jacobian(bot.structure,λ)
 𝒦a = transpose(Ň)*Ǩa*Ň |> Symmetric 
@@ -1924,7 +1994,7 @@ struct𝒦p = [
         # @show s
         λ = inv(Ǎ*transpose(Ǎ))*Ǎ*Bᵀ*s
         # @show f,λ
-        Ǩa = - RB.cstr_forces_jacobian(bot.structure,λ)
+        Ǩa = RB.cstr_forces_jacobian(bot.structure,q,λ)
         𝒦a = transpose(Ň)*Ǩa*Ň
 
         Ǩg = RB.build_geometric_stiffness_matrix!(bot.structure,q,s)
@@ -2051,7 +2121,7 @@ with_theme(theme_pub;
     fig
 end
 
-λ = -inv(Ǎ*transpose(Ǎ))*Ǎ*Bᵀ*f
+λ = inv(Ǎ*transpose(Ǎ))*Ǎ*Bᵀ*f
 # @show f,λ
 using Symbolics
 @variables λ[1:6]
diff --git a/yard/tail/bai.jl b/yard/tail/bai.jl
index 5d46ddb..fff835a 100644
--- a/yard/tail/bai.jl
+++ b/yard/tail/bai.jl
@@ -1,32 +1,29 @@
-using LinearAlgebra
-using StaticArrays, SparseArrays, TypeSortedCollections
-using EponymTuples
-using Unitful, Match, Printf
-using GeometryBasics, Meshing
-using Rotations, CoordinateTransformations
-using Unitful
-using LaTeXStrings
-using Makie
-import GLMakie as GM
-GM.activate!()
-import GeometryBasics as GB
-using FileIO, MeshIO
-using Revise
+using Revise #jl
 import Rible as RB
 
-cd("examples/tail")
-include("../vis.jl")
-includet("../vis.jl")
+include(joinpath(pathof(RB),"../../yard/stability_stiffness.jl"))
+using AbbreviatedStackTraces #jl
+ENV["JULIA_STACKTRACE_ABBREVIATED"] = true #jl
+ENV["JULIA_STACKTRACE_MINIMAL"] = true #jl
+
+figdir::String = ""
+
+include(joinpath(pathof(RB),"../../test/vis.jl"))
+includet(joinpath(pathof(RB),"../../test/vis.jl")) #jl
+
+include(joinpath(pathof(RB),"../../examples/robots/bai.jl"))
+includet(joinpath(pathof(RB),"../../examples/robots/bai.jl")) #jl
+
 #-- preamble
 
-mesh_head = load("装配体头部-1.2.3.STL") |> make_patch(;trans = [10, 0, 0],rot=RotZYX(π/2,-π/2,π/2))
-mesh_rib3 = load("肋片-3.STL") |> make_patch(;trans = [43+10, 0, 0], rot=RotZYX(π/2,-π/2,π/2))
+mesh_head = load(RB.assetpath("tail/装配体头部-1.2.3.STL")) |> RB.make_patch(;trans = [10, 0, 0],rot=RotZYX(π/2,-π/2,π/2))
+mesh_rib3 = load(RB.assetpath("tail/肋片-3.STL")) |> RB.make_patch(;trans = [43+10, 0, 0], rot=RotZYX(π/2,-π/2,π/2))
 mesh_headrib = GB.merge([mesh_head,mesh_rib3])
-mesh_rib4 = load("肋片-4.STL") |> make_patch(;rot=RotZYX(π/2,-π/2,π/2))
-mesh_rib5 = load("肋片-5 - 副本.STL") |> make_patch(;rot=RotZYX(π/2,-π/2,π/2))
-mesh_rib7 = load("肋片-7 - 副本.STL") |> make_patch(;rot=RotZYX(π/2,-π/2,π/2))
-mesh_fin = load("尾鳍-v1-软.STL") |> make_patch(;rot=RotZYX(π/2,-π/2,π/2))
-mesh_bar = load("摆杆-v1.2.2.STL") |> make_patch(;rot=RotZYX(π/2,-π/2,π/2))
+mesh_rib4 = load(RB.assetpath("tail/肋片-4.STL")) |> RB.make_patch(;rot=RotZYX(π/2,-π/2,π/2))
+mesh_rib5 = load(RB.assetpath("tail/肋片-5 - 副本.STL")) |> RB.make_patch(;rot=RotZYX(π/2,-π/2,π/2))
+mesh_rib7 = load(RB.assetpath("tail/肋片-7 - 副本.STL")) |> RB.make_patch(;rot=RotZYX(π/2,-π/2,π/2))
+mesh_fin = load(RB.assetpath("tail/尾鳍-v1-软.STL")) |> RB.make_patch(;rot=RotZYX(π/2,-π/2,π/2))
+mesh_bar = load(RB.assetpath("tail/摆杆-v1.2.2.STL")) |> RB.make_patch(;rot=RotZYX(π/2,-π/2,π/2))
 mesh(mesh_fin,color=:slategrey)
 meshes = [
     mesh_headrib,
@@ -64,183 +61,205 @@ meshes = [
     308.454 #摆杆-v1.2.2
 ]
 
-# 定义一个函数来新建机器人
-function make_bai(meshes,质心,质量,惯量)
-
-    nb = 6
-    ver_lengths_raw = [
-        #  0   43;
-        # 18   18;
-        0    43+18+18-10;
-        18   16;
-        19.1  9;
-        18    8;
-        18    8;
-         0   16.29;
-         0  150
-    ]
-    ver_lengths = sum(ver_lengths_raw,dims=2)
-    hor_lengths = [
-        32.61,
-        26.21,
-        20.64,
-        14.71,
-        13.24,
-        # 13.24,
-         2.5
-    ]
-    nhor = length(hor_lengths)
-    P = [
-        zeros(2,)
-        for i = 1:nhor+1
-    ]
-    for i = 2:nhor+1
-        P[i] = P[i-1] + [0.0,-ver_lengths[i-1]]
-    end
-
-    function rigidbody(i,P)
-        if i == 1
-            contactable = false
-            visible = true
-            ci = collect(1:6)
-            cstr_idx = Int[]
-        elseif i in [2,nb]
-            contactable = true
-            visible = true
-            ci = collect(1:2)
-            cstr_idx = collect(1:3)
-        else
-            contactable = true
-            visible = true
-            ci = Int[]
-            cstr_idx = collect(1:3)
-        end
-        α = -π/2
-        if i == 1
-            ri = SVector{2}(0.0,  0.0)
-            rj = SVector{2}(P[i+1] )
-        elseif i < nb
-            ri = SVector{2}(P[i])
-            rj = SVector{2}(P[i+1])
-        else            
-            ri = SVector{2}(0.0,  0.0)
-            rj = SVector{2}(0.0,-150.0)
-        end
-        b = hor_lengths[i]
-        offset_fix = begin
-            if i == 1
-                ret = 43 + 10
-            elseif i == nb
-                ret = 150
-            else
-                ret = 0
-            end
-            ret 
-        end
-        offset = ver_lengths_raw[i,1]
-        ro = ri + SVector{2}(0.0,-offset)
-        mass_locus = SVector{2}(质心[i])
-        m = 质量[i]
-        inertia = 惯量[i]/2
-        Ī = SMatrix{2,2}([
-            inertia 0
-            0 inertia
-        ])
-        r̄p1 = SVector{2}(offset_fix,-b,)
-        r̄p2 = SVector{2}(offset_fix, b,)
-        r̄p3 = SVector{2}(-offset, 0.0)
-        r̄p4 = SVector{2}(norm(rj-ri)-offset, 0.0)
-        loci = [r̄p1,r̄p2,r̄p3,r̄p4]
-        nr̄p = length(loci)
-        ṙo = zeros(2); ω = 0.0
-        prop = RB.RigidBodyProperty(
-            i,
-            contactable,
-            m,
-            Ī,
-            SVector{2}(mass_locus),
-            loci;
-            visible = visible,
-        )
-        nmcs = RB.NCF.NC2P1V(ri, rj, ro, α)
-        state = RB.RigidBodyState(prop, nmcs, ro, α, ṙo, ω, ci, cstr_idx)
-        mesh_rigid = meshes[i]
-        body = RB.RigidBody(prop, state, mesh_rigid)
-        rb
-    end
-    rbs = [
-        rigidbody(i, P) for i = 1:nb
-    ]
-    rigdibodies = TypeSortedCollection(rbs)
-    numbered = RB.number(rigdibodies)
-    matrix_sharing_raw = Vector{Matrix{Int}}()
-    for i = 1:nb-2
-        s = zeros(2, nb)
-        s[1:2, i] = 3:4
-        s[1:2, i+1] = 1:2
-        push!(matrix_sharing_raw, s)
-    end
-    s = zeros(2, nb)
-    s[1:2,  1] = 1:2
-    s[1:2, nb] = 1:2
-    push!(matrix_sharing_raw, s)
-    matrix_sharing = reduce(vcat, matrix_sharing_raw)
-    # display(matrix_sharing)
-    indexed = RB.index(rigdibodies, matrix_sharing)
-    # indexed = RB.index(rigdibodies, )
-
-    ncables = 2(nb-1)
-    original_restlens = zeros(ncables)
-    ks = zeros(ncables)
-    for i = 1:ncables
-        original_restlens[i] = 22.0
-        ks[i] = 1000.0
-    end
-    cables =
-        [RB.DistanceSpringDamper2D( original_restlens[i], ks[i], 0.0;slack=true) for i = 1:ncables]  #
-    apparatuses = (cables = cables,)
-    acs = [
-        RB.RegisterActuator(1,
-            [1:2(nb-1)],
-            original_restlens,
-        )
-    ]
-    hub = (actuators = acs,)
-
-    matrix_cnt_raw = Vector{Matrix{Int}}()
-    for i = 1:nb-1
-        s = zeros(2, nb)
-        s[1, i  ] =  1
-        s[1, i+1] = -1
-        s[2, i  ] =  2
-        s[2, i+1] = -2
-        push!(matrix_cnt_raw, s)
-    end
-    matrix_cnt = reduce(vcat, matrix_cnt_raw)
-    # display(matrix_cnt)
-    connected = RB.connect(rigdibodies, matrix_cnt)
-    tensioned = @eponymtuple(connected,)
-    cnt = RB.Connectivity(numbered, indexed, tensioned)
-
-    st = RB.Structure(rigdibodies, apparatuses, cnt)
-    bot = RB.Robot(st, hub)
-end
-
 tail = make_bai(meshes,质心,质量,惯量)
 
+bot = tail
+botvis = deepcopy(bot)
 plot_traj!(
-    tail;
+    botvis;
     fontsize = 14 |> pt2px,
-    showmesh = false,
+    show_axis = false,
+    showmesh = true,
+    showpoints = false,
+    showlabels = false,
     xlims = (-50,50),
     ylims = (-300,40),
     showground = false
 )
 
 # 静力平衡
-RB.check_static_equilibrium_output_multipliers(tail.st;)
+RB.check_static_equilibrium_output_multipliers(bot.structure;)
 
 # 刚度、稳定性分析
-RB.undamped_eigen(tail.st)
+ω²,δq̌ = RB.undamped_eigen(bot.structure)
+
+## δq̌ = [Ň*orthovm[:,i] for i in axes(orthovm,2)]
+scaling=0.2
+nk = 5
+RB.reset!(botvis)
+for i = 1:nk
+    push!(botvis.traj,deepcopy(botvis.traj[end]))
+    botvis.traj.t[end] = i
+    δq̌i = δq̌[i]
+    ratio = norm(δq̌i)/norm(botvis.traj.q̌[begin])
+    botvis.traj.q̌[end] .= botvis.traj.q̌[begin] .+ scaling.*δq̌i/ratio
+end
+with_theme(theme_pub;
+        fontsize = 16,
+        Axis3 = (
+            azimuth = 4.7078743833269865,
+            elevation = 1.307620956698724
+        )
+    ) do
+    fig = Figure()
+    gd2 = fig[1,1:nk] = GridLayout()
+    plot_traj!(
+        botvis;
+        fig = gd2,
+        AxisType=Axis3,
+        showinfo = true,
+        gridsize=(1, nk),
+        atsteps=2:nk+1,
+        doslide=false,
+        showmesh = false,
+        showwire = false,
+        showlabels=true,
+        showpoints=true,
+        # showcables = false,
+        showground = false,
+        xlims = (-150,150),
+        ## ylims = (-400,300),
+        ylims = (-400,0),
+        zlims = (-1,0),
+        slack_linestyle = :solid,
+        ## showinit = true,
+        refcolor = Makie.RGBA{Float32}(0.5,0.5,0.5,0.5),
+        titleformatfunc = (sgi,tt)-> begin
+            rich(
+                    rich("($(alphabet[sgi+1])) ", font=:bold),
+                    "Mechanism Mode $sgi"
+                )
+        end,
+        sup! = (ax,tgob,sgi)->begin
+            bodies = RB.get_bodies(tgob[])
+            r1p3 = bodies[1].state.loci_states[3].frame.position
+            r1p4 = bodies[1].state.loci_states[4].frame.position
+            r2p4 = bodies[2].state.loci_states[4].frame.position
+            r3p4 = bodies[3].state.loci_states[4].frame.position
+            r4p4 = bodies[4].state.loci_states[4].frame.position
+            r5p4 = bodies[5].state.loci_states[4].frame.position
+            r5g = bodies[5].state.mass_locus_state.frame.position
+            @show r1p3, r1p4
+            lines!(ax,
+                [
+                    r1p3,r1p4,r2p4,r3p4,r4p4,r5p4,r5g
+                ]
+            )
+            ## hidedecorations!(ax)
+            ## xlims!(ax,-50,50)
+            ## RB.hidez(ax)
+            ## ylims!(ax,-300,40)
+        end,
+    )
+    fig
+end
+
+
+# 动力学仿真问题
+prob = RB.DynamicsProblem(bot,)
+
+# 动力学仿真求解
+RB.solve!(prob,
+    RB.DynamicsSolver(
+        RB.Zhong06()
+    );
+    dt=0.001,tspan=(0.0,100.0),ftol=1e-10,verbose=true
+)
+
+plot_traj!(bot)
+
+
+using BlockDiagonals
+
+body1 = RB.get_bodies(bot)[1]
+
+ke = RB.get_kinetic_energy!(bot,2,)
+kec = RB.get_kinetic_energy_coords!(bot,2)
+(ke.-kec)./kec
+
+β = vcat(
+    -Ω×(MS*Ω) + Ω×(M*V),
+    Ω×(I*Ω) + MS*(Ω×V)
+)
+
+# Function for first forward recursion on kinematics
+function forward_recursion_kinematics(Xsj, lj, Vj, ηj)
+    # Initialize velocities
+    Vj_plus_1 = zeros(3)
+    ηj_plus_1 = zeros(3)
+
+    # Iterate over segment slices
+    for i in 1:size(Xsj, 2)-1
+        ΔX = Xsj[:, i+1] - Xsj[:, i]
+        Vj_plus_1 += Vj[:, i] + cross(ΔX, ηj[:, i])
+        ηj_plus_1 += ηj[:, i]
+    end
+
+    return Vj_plus_1, ηj_plus_1
+end
+
+# Function for backward recursion on external dynamics
+function backward_recursion_external(Mfj, Ifj, η, ηj, Xsj, lj, external_force)
+    # Initialize forces and torques
+    external_force = reverse(external_force, dims=2) # Reverse external force for backward recursion
+    F_ext = zeros(3)
+    τ_ext = zeros(3)
+    
+    # Iterate over segment slices in reverse order
+    for i in size(Xsj, 2):-1:1
+        # Compute external forces and torques
+        ΔX = Xsj[:, i+1] - Xsj[:, i]
+        F_ext += external_force[:, i] * norm(ΔX)
+        τ_ext += cross(Xsj[:, i], external_force[:, i]) * norm(ΔX)
+    end
+    
+    # Compute fluid forces at endpoints
+    f_pres_0 = zeros(3)
+    f_pres_lj = cross(Xsj[:, end], Pfj[:, end])
+    
+    # Compute linear and angular dynamics
+    dPf_dt = Mfj * (Vj_plus_1 - Vj[:, end]) - F_ext + f_pres_0 - f_pres_lj
+    dβf_dt = Ifj * (ηj_plus_1 - ηj[:, end]) - τ_ext + cross(Xsj[:, end], f_pres_lj)
+    
+    return dPf_dt, dβf_dt
+end
+
+# Function for second forward recursion on internal dynamics
+function forward_recursion_internal(dPf_dt, dβf_dt, Mfj, Ifj, Xsj, lj)
+    # Initialize forces and torques
+    Pfj = zeros(3, size(Xsj, 2))
+    Pfj[:, end] = Mfj * Vj_plus_1
+    βfj = zeros(3, size(Xsj, 2))
+    βfj[:, end] = Ifj * ηj_plus_1
+    
+    # Iterate over segment slices
+    for i in 1:size(Xsj, 2)-1
+        ΔX = Xsj[:, i+1] - Xsj[:, i]
+        Pfj[:, i] = Pfj[:, i+1] + dPf_dt * norm(ΔX)
+        βfj[:, i] = βfj[:, i+1] + dβf_dt * norm(ΔX)
+    end
+    
+    return Pfj, βfj
+end
+
+# Example inputs
+Xsj = rand(3, 10) # Segment slice positions
+lj = rand(10) # Segment lengths
+Vj = rand(3, 10) # Segment velocities
+ηj = rand(3, 10) # Segment angular velocities
+Mfj = rand(3, 3) # Fluid added mass tensor
+Ifj = rand(3, 3) # Fluid added inertia tensor
+external_force = rand(3, 10) # External forces
+
+# First forward recursion on kinematics
+Vj_plus_1, ηj_plus_1 = forward_recursion_kinematics(Xsj, lj, Vj, ηj)
+
+# Backward recursion on external dynamics
+dPf_dt, dβf_dt = backward_recursion_external(Mfj, Ifj, η, ηj, Xsj, lj, external_force)
+
+# Second forward recursion on internal dynamics
+Pfj, βfj = forward_recursion_internal(dPf_dt, dβf_dt, Mfj, Ifj, Xsj, lj)
+
+println("Linear fluid dynamics (Pfj): ", Pfj)
+println("Angular fluid dynamics (βfj): ", βfj)
 
-# 动力学分析
diff --git a/yard/tail/curve.jl b/yard/tail/curve.jl
deleted file mode 100644
index 7b605da..0000000
--- a/yard/tail/curve.jl
+++ /dev/null
@@ -1,23 +0,0 @@
-using NLsolve
-using Makie
-AbstractPlotting.__init__()
-n = 12
-function on_curve(n,d,f)
-    x = zeros(n)
-    y = zeros(n)
-    y[1] = f(x[1])
-    for i = 2:n
-        function func!(func,unk)
-            xi,yi = unk
-            dy = yi - y[i-1]
-            dx = xi - x[i-1]
-            func[1] = dy^2 + dx^2 - d
-            func[2] = yi-f(xi)
-        end
-        soli = nlsolve(func!,[x[i-1]+d,f(x[i-1]+d)])
-        x[i],y[i] = soli.zero
-    end
-    x,y
-end
-x,y = on_curve(12,0.1,sin)
-plot(x,y)
diff --git a/yard/tail/dynamics.jl b/yard/tail/dynamics.jl
index 8fc814d..67b2dd3 100644
--- a/yard/tail/dynamics.jl
+++ b/yard/tail/dynamics.jl
@@ -189,7 +189,7 @@ function dynfuncs(bot)
         RB.clear_forces!(st)
         RB.update_bodies!(st,q,q̇)
         RB.update_apparatuses!(st)
-        ## RB.apply_gravity!(st)
+        ## RB.apply_field!(st)
         RB.assemble_forces!(st)
         RB.get_force!(F,st)
     end
diff --git a/yard/tail/plotting.jl b/yard/tail/plotting.jl
deleted file mode 100644
index b54875c..0000000
--- a/yard/tail/plotting.jl
+++ /dev/null
@@ -1,65 +0,0 @@
-rb_bars(body) = [Point(body.state.loci_states[1]) => Point(body.state.loci_states[2]);]
-
-function bars_and_cables(tgstruct)
-    bars = [Node(rb_bars(body)) for rb in tgstruct.rigidbodies]
-    cables = Node(RB.get_cables(tgstruct))
-    bars, cables
-end
-
-function plotstructure!(ax,tgstruct)
-    bars, cables = bars_and_cables(tgstruct)
-    function plot!(ax,bars,cables)
-        for (lineid,line) in enumerate(bars)
-            linesegments!(ax, line, color = :black, linewidth = 4)
-        end
-        linesegments!(ax, cables, color = :deepskyblue, linewidth = 2)
-    end
-    plot!(ax,bars,cables)
-    xlims!(ax,[-0.2,0.2])
-    ylims!(ax,[-0.6,0.05])
-    bars,cables
-end
-
-function update_scene!(st,bars,cables,q)
-    cnt = st.connectivity
-    RB.distribute_q_to_rbs!(st,q)
-    for (id,rb) in enumerate(st.rigidbodies)
-        bars[id][] = rb_bars(body)
-    end
-    cables[] = RB.get_cables(st)
-    # angles = update_angles(st)
-    # @show angles
-end
-
-function sliderplot(fig,tr,ax,cables)
-    @unpack st, traj = tr
-    ls_step = labelslider!(fig,"step",1:length(traj.ts))
-    fig[2,1] = ls_step.layout
-    on(ls_step.slider.value) do this_step
-        # analyse_slackness(st,sol.qs[this_step])
-        update_scene!(st,ax,cables,traj.qs[this_step])
-        # show_camera(ax.scene)
-    end
-end
-function recordplot(fig,tr,ax,cables;record_name)
-    @unpack st, traj = tr
-    record(fig, record_name, 1:length(traj.ts); framerate = 30) do this_step
-        update_scene!(st,ax,cables,traj.qs[this_step])
-        # update_cam!(ax.scene, eyepos,lookat)
-    end
-end
-
-
-function plotstructure(tr::RB.Robot;record_name="")
-    @unpack st, traj = tr
-    fig = Figure(resolution=(1000,1000))
-    ax = fig[1, 1] = Axis(fig)
-    bars,cables = plotstructure!(ax,st)
-    ax.aspect = DataAspect()
-    if record_name==""
-        sliderplot(fig,tr,ax,cables)
-    else
-        recordplot(fig,tr,ax,cables;record_name)
-    end
-    fig
-end
diff --git a/yard/tail/swing.jl b/yard/tail/swing.jl
deleted file mode 100644
index 06f95d8..0000000
--- a/yard/tail/swing.jl
+++ /dev/null
@@ -1,74 +0,0 @@
-using LinearAlgebra
-using SparseArrays
-using Parameters
-using StaticArrays
-# using BenchmarkTools
-# import PyPlot; const plt = PyPlot
-# using LaTeXStrings
-using Makie
-AbstractPlotting.__init__()
-
-using NLsolve
-using Revise
-using TensegritySolvers; const TS = TensegritySolvers
-using Robot
-const TR = Robot
-include("tail_define.jl")
-include("plotting.jl")
-
-include("../analysis.jl")
-
-n = 13
-tail = make_curve_tail(n)
-# Vy = make_curve_tail(n)
-_,_,tailscene = plotstructure(tail)
-tailscene
-# @code_warntype make_tail(n)
-q0,q̇0,λ0 = RB.get_initial(tail)
-# q̇0[end-3:end] .= 5*[0.1,0.0,0.1,0.0]
-
-# RB.get_nbodycstr(tail)
-# RB.get_nbodydof(tail) # RB.get_num_of_coords(tail)
-
-
-function dynfuncs(tgstruct,q0)
-
-    M = RB.build_massmatrix(tgstruct)
-    Φ = RB.build_Φ(tgstruct,q0)
-    A = RB.build_A(tgstruct)
-
-    #Q̃=RB.build_Q̃(tgstruct)
-
-    function F!(F,q,q̇,t)
-        # du = 0.01*sin(t)
-        # RB.actuate!(tgstruct.actuators[1],du)
-        RB.reset_forces!(tgstruct)
-        RB.distribute_q_to_rbs!(tgstruct,q,q̇)
-        RB.update_cables_apply_forces!(tgstruct)
-        # RB.apply_gravity!(tgstruct,factor=0.001)
-        # F .= Q̃*RB.fvector(tgstruct)
-        F .= 0.0
-        RB.assemble_forces!(F,tgstruct)
-    end
-
-    M,Φ,A,F!,nothing
-end
-
-M,Φ,A,F!,Jacs = dynfuncs(tail,q0)
-# Φ(q0)
-# @code_warntype Φ(q0)
-# A(q0)
-# @code_warntype A(q0)
-F = similar(q0)
-# @code_warntype F!(F,q0,q̇0,0.0)
-dt = 0.01
-prob = TS.DyProblem(dynfuncs(tail,q0),q0,q̇0,λ0,(0.0,10.0))
-sol = TS.solve(prob,TS.Zhong06(),dt=dt,ftol=1e-14,verbose=true)
-
-plotstructure(tail,sol,sliderplot)
-plotstructure(tail,sol,recordplot)
-# sol = TS.solve(prob,TS.ConstSPARK(1),dt=dt,ftol=1e-12,verbose=true)
-@code_warntype TS.solve(prob,TS.Zhong06(),dt=dt,ftol=1e-14,verbose=true)
-kes,epes,gpes,restitution_coefficients,es_err = analyse_energy(tail,sol;gravity=false,elasticity=true)
-restitution_coefficients
-plot(restitution_coefficients)
diff --git a/yard/tail/test.jl b/yard/tail/test.jl
deleted file mode 100644
index a20d694..0000000
--- a/yard/tail/test.jl
+++ /dev/null
@@ -1,53 +0,0 @@
-using LinearAlgebra
-using SparseArrays
-using Parameters
-using StaticArrays
-using Makie
-#plot(rand(10))
-# using BenchmarkTools
-# import PyPlot; const plt = PyPlot
-# using LaTeXStrings
-# using NLsolve
-using Revise
-using Rible; const TR = Rible
-#cd("examples/tail")
-includet("tail_define.jl")
-includet("make_new_tail.jl")
-includet("plotting.jl")
-includet("../analysis.jl")
-
-n = 4
-tail = make_new_tail(n)
-#plotstructure(tail)
-#@code_warntype make_tail(n)
-q0,q̇0,λ0 = RB.get_initial(tail.st)
-q0=q =Vec{28,Float64}(0,0,2,0,0,-2,2,-2,0,-4,2,-4,0,-6,2,-6,0,-8,2,-8,0,-10,0,-12,0,-14,0,-16)
-q̇0[end-3:end] .= 5*[0.1,0.0,0.1,0.0]
-RB.set_new_initial!(tail,q0,q̇0)
-
-function dynfuncs(tr)
-    @unpack st = tr
-    M = RB.build_massmatrix(st)
-    Φ = RB.build_Φ(st)
-    A = RB.build_A(st)
-
-    #Q̃=RB.build_Q̃(st)
-
-    function F!(F,q,q̇,t)
-        RB.reset_forces!(st)
-        RB.distribute_q_to_rbs!(st,q,q̇)
-        RB.update_cables_apply_forces!(st)
-        # F .= Q̃*RB.fvector(tgstruct)
-        F .= 0.0
-        RB.assemble_forces!(F,st)
-    end
-
-    M,Φ,A,F!,nothing
-end
-
-dt = 0.01
-prob = RB.DyProblem(dynfuncs(tail),tail,(0.0,20.0))
-RB.solve!(tail,prob,RB.Zhong06(),dt=dt,ftol=1e-14,verbose=true)
-# sol = RB.solve(prob,RB.ConstSPARK(1),dt=dt,ftol=1e-12,verbose=true)
-#@code_warntype RB.solve(prob,RB.Zhong06(),dt=dt,ftol=1e-14,verbose=true)
-plotstructure(tail)