Fix tendon length expression and actuation matrix for planar pcs system

pyproject.toml

@@ -17,7 +17,7 @@ name = "jsrm"  # Required
 #
 # For a discussion on single-sourcing the version, see
 # https://packaging.python.org/guides/single-sourcing-package-version/
-version = "0.0.15"  # Required
+version = "0.0.16"  # Required
 
 # This is a one-line description or tagline of what your project does. This
 # corresponds to the "Summary" metadata field:

src/jsrm/symbolic_derivation/planar_pcs.py

@@ -153,7 +153,7 @@ def symbolically_derive_planar_pcs_model(
         U_g = U_g + U_gi
 
         # simplify derived tendon length
-        L_tend = L_tend + s * (1 + kappa_be * d) * sp.sqrt(sigma_sh**2 + sigma_ax**2)
+        L_tend = L_tend + s * sp.sqrt(sigma_sh**2 + (sigma_ax + kappa_be * d)**2)
         L_tend_sms.append(L_tend)
         print(f"L_tend of segment {i+1}:\n", L_tend)
         # take the derivative of the tendon length with respect to the configuration

src/jsrm/systems/tendon_actuated_planar_pcs.py

@@ -87,11 +87,11 @@ def compute_A_d_wrt_xi_i(i: Array, l_i: Array, xi_i: Array) -> Array:
                     Returns:
                         A_d_segment: actuation matrix for the segment of shape (3, 3)
                     """
-                    sigma_norm = jnp.sqrt(xi_i[1] ** 2 + xi_i[2] ** 2)
+                    square_root_term = jnp.sqrt(xi[1]**2 + (xi[2] + d * xi[0])**2)
                     A_d_wrt_xi_i = - jnp.array([
-                        d * l_i * sigma_norm,
-                        l_i * xi_i[1] * (1 + d * xi_i[0]) / sigma_norm,
-                        l_i * xi_i[2] * (1 + d * xi_i[0]) / sigma_norm,
+                        l_i * d * (d * xi_i[0] + xi_i[2]) / square_root_term,
+                        l_i * xi_i[1] / square_root_term,
+                        l_i * (d * xi_i[0] + xi_i[2]) / square_root_term,
                     ])
                     return jnp.where(
                         i * jnp.ones((3, )) <= segment_idx * jnp.ones((3, )),