Fix tendon length expression and actuation matrix for planar pcs system

Author: mstoelzle

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, )),