Fix sign of gravitational potential energy

Author: mstoelzle

src/jsrm/symbolic_derivation/planar_hsa.py

@@ -219,7 +219,7 @@ def symbolically_derive_planar_hsa_model(
     # inertia matrix
     B = sp.zeros(num_dof, num_dof)
     # potential energy
-    U = sp.Matrix([[0]])
+    U_g = sp.Matrix([[0]])
     # stiffness matrix
     Shat = sp.zeros(3 * num_segments, 3 * num_segments)
     # elastic vector
@@ -299,11 +299,11 @@ def symbolically_derive_planar_hsa_model(
             B = B + Br_ij
 
             # derivative of the potential energy with respect to the point coordinate s
-            dUr_ds = sp.simplify(rhor[i, j] * Ar[i, j] * g.T @ pr)
+            dU_g_r_ds = sp.simplify(-rhor[i, j] * Ar[i, j] * g.T @ pr)
             # potential energy of the current segment
-            U_ij = sp.simplify(sp.integrate(dUr_ds, (s, 0, l[i])))
+            U_g_ij = sp.simplify(sp.integrate(dU_g_r_ds, (s, 0, l[i])))
             # add potential energy of segment to previous segments
-            U = U + U_ij
+            U_g = U_g + U_g_ij
 
             # strains in physical HSA rod
             pxir = _sym_beta_fn(vxi, roff[i, j])
@@ -466,8 +466,8 @@ def symbolically_derive_planar_hsa_model(
             Bp = sp.simplify(Bp)
         B = B + Bp
         # potential energy of the platform
-        Up = sp.simplify(mp * g.T @ pCoGp)
-        U = U + Up
+        U_g_p = sp.simplify(-mp * g.T @ pCoGp)
+        U_g = U_g + U_g_p
 
         # update the orientation for the next segment
         th_prev = th.subs(s, l[i])
@@ -507,8 +507,8 @@ def symbolically_derive_planar_hsa_model(
         Bpl = sp.simplify(Bpl)
     B = B + Bpl
     # add the gravitational potential energy of the payload
-    Upl = sp.simplify(mpl * g.T @ pCoGpl)
-    U = U + Upl
+    Upl = sp.simplify(- mpl * g.T @ pCoGpl)
+    U_g = U_g + Upl
 
     # simplify mass matrix
     if simplify:
@@ -519,7 +519,7 @@ def symbolically_derive_planar_hsa_model(
     print("C =\n", C)
 
     # compute the gravity force vector
-    G = -U.jacobian(xi).transpose()
+    G = U_g.jacobian(xi).transpose()
     if simplify:
         G = sp.simplify(G)
     print("G =\n", G)

src/jsrm/symbolic_derivation/planar_pcs.py

@@ -64,7 +64,7 @@ def symbolically_derive_planar_pcs_model(
     # inertia matrix
     B = sp.zeros(num_dof, num_dof)
     # potential energy
-    U = sp.Matrix([[0]])
+    U_g = sp.Matrix([[0]])
 
     # symbol for the point coordinate
     s = sp.symbols("s", real=True, nonnegative=True)
@@ -134,15 +134,15 @@ def symbolically_derive_planar_pcs_model(
         B = B + B_i
 
         # derivative of the potential energy with respect to the point coordinate s
-        dU_ds = rho[i] * A[i] * g.T @ p
+        dU_g_ds = -rho[i] * A[i] * g.T @ p
         if simplify_expressions:
-            dU_ds = sp.simplify(dU_ds)
-        # potential energy of the current segment
-        U_i = sp.integrate(dU_ds, (s, 0, l[i]))
+            dU_g_ds = sp.simplify(dU_g_ds)
+        # gravitational potential energy of the current segment
+        U_gi = sp.integrate(dU_g_ds, (s, 0, l[i]))
         if simplify_expressions:
-            U_i = sp.simplify(U_i)
+            U_gi = sp.simplify(U_gi)
         # add potential energy of segment to previous segments
-        U = U + U_i
+        U_g = U_g + U_gi
 
         # update the orientation for the next segment
         th_prev = th.subs(s, l[i])
@@ -155,11 +155,11 @@ def symbolically_derive_planar_pcs_model(
         B = sp.simplify(B)
     print("B =\n", B)
 
-    C = compute_coriolis_matrix(B, xi, xi_d)
+    C = compute_coriolis_matrix(B, xi, xi_d, simplify=simplify_expressions)
     print("C =\n", C)
 
     # compute the gravity force vector
-    G = -U.jacobian(xi).transpose()
+    G = U_g.jacobian(xi).transpose()
     if simplify_expressions:
         G = sp.simplify(G)
     print("G =\n", G)
@@ -190,7 +190,7 @@ def symbolically_derive_planar_pcs_model(
             "B": B,  # mass matrix
             "C": C,  # coriolis matrix
             "G": G,  # gravity vector
-            "U": U,  # gravitational potential energy
+            "U_g": U_g,  # gravitational potential energy
         },
     }
 

src/jsrm/systems/planar_pcs.py

@@ -132,8 +132,8 @@ def select_params_for_lambdify_fn(params: Dict[str, Array]) -> List[Array]:
     G_lambda = sp.lambdify(
         params_syms_cat + sym_exps["state_syms"]["xi"], sym_exps["exps"]["G"], "jax"
     )
-    U_lambda = sp.lambdify(
-        params_syms_cat + sym_exps["state_syms"]["xi"], sym_exps["exps"]["U"], "jax"
+    U_g_lambda = sp.lambdify(
+        params_syms_cat + sym_exps["state_syms"]["xi"], sym_exps["exps"]["U_g"], "jax"
     )
 
     compute_stiffness_matrix_for_all_segments_fn = vmap(
@@ -299,10 +299,9 @@ def dynamical_matrices_fn(
         Ib = A**2 / (4 * jnp.pi)
 
         # elastic and shear modulus
-        E, G = params["E"], params["G"]
-
+        elastic_modulus, shear_modulus = params["E"], params["G"]
         # stiffness matrix of shape (num_segments, 3, 3)
-        S = compute_stiffness_matrix_for_all_segments_fn(A, Ib, E, G)
+        S = compute_stiffness_matrix_for_all_segments_fn(A, Ib, elastic_modulus, shear_modulus)
 
         # we define the elastic matrix of shape (n_xi, n_xi) as K(xi) = K @ xi where K is equal to
         K = blk_diag(S)
@@ -375,7 +374,7 @@ def potential_energy_fn(params: Dict[str, Array], q: Array, eps: float = 1e4 * g
 
         # gravitational potential energy
         params_for_lambdify = select_params_for_lambdify_fn(params)
-        U_G = U_lambda(*params_for_lambdify, *xi_epsed)
+        U_G = U_g_lambda(*params_for_lambdify, *xi_epsed)
 
         # total potential energy
         U = (U_G + U_K).squeeze()