Implement operational-space dynamics for planar pcs and fix gravitational potential energy expression (#4)

* Start implementing operational space dynamics for planar pcs

* Fix sign of gravitational potential energy

* Add more plotting to the planar pcs simulation example

* Improve labels for simulation data analysis plots

* Do not simplify symbolic expressions if we are deriving dynamics for more than 2 segments

* Add more symbolic expressions for planar pcs

* Formatting

* Add symbolic expression for 5 segments

* Implement `operational_space_selector` into `operational_space_dynamical_matrices_fn` for planar pcs

* Make `operational_space_dynamical_matrices_fn` jittable again by specifying `operational_space_selector` as a tuple

* Add `stiffness` function to planar pcs system

* Run formatting

* Bump version and start testing on Python 3.13

Author: mstoelzle

.github/workflows/release.yml

@@ -13,9 +13,9 @@ jobs:
     runs-on: ubuntu-latest
     steps:
     - name: Checkout
-      uses: actions/checkout@v2
+      uses: actions/checkout@v4
     - name: Set up Python
-      uses: actions/setup-python@v1
+      uses: actions/setup-python@v5
       with:
         python-version: '3.x'
     - name: Install build dependencies

.github/workflows/test.yml

@@ -9,14 +9,14 @@ jobs:
   test:
     strategy:
       matrix:
-        python: ['3.10', '3.11', '3.12']
+        python: ['3.10', '3.11', '3.12', '3.13']
         platform: [ubuntu-latest, macos-latest, windows-latest]
     runs-on: ${{ matrix.platform }}
     steps:
     - name: Checkout
-      uses: actions/checkout@v3
+      uses: actions/checkout@v4
     - name: Set up Python ${{ matrix.python }}
-      uses: actions/setup-python@v3
+      uses: actions/setup-python@v5
       with:
         python-version: ${{ matrix.python }}
     - name: Install test dependencies

examples/analyze_segment_fusion.py

@@ -1,8 +1,12 @@
 import sympy as sp
 
-l1, l2 = sp.symbols('l1 l2', nonnegative=True, nonzero=True)
-kappa_be1, sigma_sh1, sigma_ax1 = sp.symbols('kappa_be1 sigma_sh1 sigma_ax1', real=True, nonnegative=True, nonzero=True)
-kappa_be2, sigma_sh2, sigma_ax2 = sp.symbols('kappa_be2 sigma_sh2 sigma_ax2', real=True, nonnegative=True, nonzero=True)
+l1, l2 = sp.symbols("l1 l2", nonnegative=True, nonzero=True)
+kappa_be1, sigma_sh1, sigma_ax1 = sp.symbols(
+    "kappa_be1 sigma_sh1 sigma_ax1", real=True, nonnegative=True, nonzero=True
+)
+kappa_be2, sigma_sh2, sigma_ax2 = sp.symbols(
+    "kappa_be2 sigma_sh2 sigma_ax2", real=True, nonnegative=True, nonzero=True
+)
 
 # deactivate shear and axial strains
 # sigma_sh1, sigma_sh2 = 0.0, 0.0
@@ -19,30 +23,42 @@
 # apply the forward kinematics on the individual segments
 s_be1, c_be1 = sp.sin(kappa_be1 * l1), sp.cos(kappa_be1 * l1)
 s_be2, c_be2 = sp.sin(kappa_be2 * l2), sp.cos(kappa_be2 * l2)
-chi1 = sp.Matrix([
-    [sigma_sh1 * s_be1 / kappa_be1 + sigma_ax1 * (c_be1 - 1) / kappa_be1],
-    [sigma_sh1 * (1 - c_be1) / kappa_be1 + sigma_ax1 * s_be1 / kappa_be1],
-    [kappa_be1 * l1],
-])
+chi1 = sp.Matrix(
+    [
+        [sigma_sh1 * s_be1 / kappa_be1 + sigma_ax1 * (c_be1 - 1) / kappa_be1],
+        [sigma_sh1 * (1 - c_be1) / kappa_be1 + sigma_ax1 * s_be1 / kappa_be1],
+        [kappa_be1 * l1],
+    ]
+)
 # rotation matrix
-R1 = sp.Matrix([
-    [c_be1, -s_be1, 0],
-    [s_be1, c_be1, 0],
-    [0, 0, 1],
-])
-chi2_rel = sp.Matrix([
-    [sigma_sh2 * s_be2 / kappa_be2 + sigma_ax2 * (c_be2 - 1) / kappa_be2],
-    [sigma_sh2 * (1 - c_be2) / kappa_be2 + sigma_ax2 * s_be2 / kappa_be2],
-    [kappa_be2 * l2],
-])
+R1 = sp.Matrix(
+    [
+        [c_be1, -s_be1, 0],
+        [s_be1, c_be1, 0],
+        [0, 0, 1],
+    ]
+)
+chi2_rel = sp.Matrix(
+    [
+        [sigma_sh2 * s_be2 / kappa_be2 + sigma_ax2 * (c_be2 - 1) / kappa_be2],
+        [sigma_sh2 * (1 - c_be2) / kappa_be2 + sigma_ax2 * s_be2 / kappa_be2],
+        [kappa_be2 * l2],
+    ]
+)
 chi2 = sp.simplify(chi1 + R1 @ chi2_rel)
 # print("chi2 =\n", chi2)
 # apply the one-segment inverse kinematic on the distal end of the two segments
 px, py, th = chi2[0, 0], chi2[1, 0], chi2[2, 0]
 s_th, c_th = sp.sin(th), sp.cos(th)
-qfkik = sp.simplify(th / (2 * (l1 + l2)) * sp.Matrix([
-    [2],
-    [py - px * s_th / (c_th - 1)],
-    [-px - py * s_th / (c_th - 1)],
-]))
+qfkik = sp.simplify(
+    th
+    / (2 * (l1 + l2))
+    * sp.Matrix(
+        [
+            [2],
+            [py - px * s_th / (c_th - 1)],
+            [-px - py * s_th / (c_th - 1)],
+        ]
+    )
+)
 print("qfkik =\n", qfkik)

examples/demo_planar_hsa_motor2ee_jacobian.py

@@ -26,7 +26,9 @@
 )
 
 
-def factory_fn(params: Dict[str, Array], verbose: bool = False) -> Tuple[Callable, Callable]:
+def factory_fn(
+    params: Dict[str, Array], verbose: bool = False
+) -> Tuple[Callable, Callable]:
     """
     Factory function for the planar HSA.
     Args:
@@ -45,7 +47,9 @@ def factory_fn(params: Dict[str, Array], verbose: bool = False) -> Tuple[Callabl
         sys_helpers,
     ) = planar_hsa.factory(sym_exp_filepath, strain_selector)
     dynamical_matrices_fn = partial(dynamical_matrices_fn, params)
-    forward_kinematics_end_effector_fn = jit(partial(forward_kinematics_end_effector_fn, params))
+    forward_kinematics_end_effector_fn = jit(
+        partial(forward_kinematics_end_effector_fn, params)
+    )
     jacobian_end_effector_fn = jit(partial(jacobian_end_effector_fn, params))
 
     def residual_fn(q: Array, phi: Array) -> Array:
@@ -64,7 +68,9 @@ def residual_fn(q: Array, phi: Array) -> Array:
     print("Compiling jac_residual_fn...")
     print(jac_residual_fn(jnp.zeros((3,)), jnp.zeros((2,))))
 
-    def phi2q_static_model_fn(phi: Array, q0: Array = jnp.zeros((3, ))) -> Tuple[Array, Dict[str, Array]]:
+    def phi2q_static_model_fn(
+        phi: Array, q0: Array = jnp.zeros((3,))
+    ) -> Tuple[Array, Dict[str, Array]]:
         """
         A static model mapping the motor angles to the planar HSA configuration using scipy.optimize.minimize.
         Arguments:
@@ -82,7 +88,12 @@ def phi2q_static_model_fn(phi: Array, q0: Array = jnp.zeros((3, ))) -> Tuple[Arr
             options={"disp": True} if verbose else None,
         )
         if verbose:
-         print("Optimization converged after", sol.nit, "iterations with residual", sol.fun)
+            print(
+                "Optimization converged after",
+                sol.nit,
+                "iterations with residual",
+                sol.fun,
+            )
 
         # configuration that minimizes the residual
         q = jnp.array(sol.x)
@@ -95,7 +106,9 @@ def phi2q_static_model_fn(phi: Array, q0: Array = jnp.zeros((3, ))) -> Tuple[Arr
 
         return q, aux
 
-    def phi2chi_static_model_fn(phi: Array, q0: Array = jnp.zeros((3, ))) -> Tuple[Array, Dict[str, Array]]:
+    def phi2chi_static_model_fn(
+        phi: Array, q0: Array = jnp.zeros((3,))
+    ) -> Tuple[Array, Dict[str, Array]]:
         """
         A static model mapping the motor angles to the planar end-effector pose.
         Arguments:
@@ -110,7 +123,9 @@ def phi2chi_static_model_fn(phi: Array, q0: Array = jnp.zeros((3, ))) -> Tuple[A
         aux["chi"] = chi
         return chi, aux
 
-    def jac_phi2chi_static_model_fn(phi: Array, q0: Array = jnp.zeros((3, ))) -> Tuple[Array, Dict[str, Array]]:
+    def jac_phi2chi_static_model_fn(
+        phi: Array, q0: Array = jnp.zeros((3,))
+    ) -> Tuple[Array, Dict[str, Array]]:
         """
         Compute the Jacobian between the actuation space and the task-space.
         Arguments:
@@ -157,17 +172,12 @@ def jac_phi2chi_static_model_fn(phi: Array, q0: Array = jnp.zeros((3, ))) -> Tup
                 phi = jnp.array([1.0, 1.0])
             case _:
                 rng, subkey = random.split(rng)
-                phi = random.uniform(
-                    subkey,
-                    phi_max.shape,
-                    minval=0.0,
-                    maxval=phi_max
-                )
+                phi = random.uniform(subkey, phi_max.shape, minval=0.0, maxval=phi_max)
 
         print("i", i)
 
         chi, aux = phi2chi_static_model_fn(phi, q0=q0)
         print("phi", phi, "q", aux["q"], "chi", chi)
 
         J_phi2chi, aux = jac_phi2chi_static_model_fn(phi, q0=q0)
-        print("J_phi2chi:\n", J_phi2chi)
+        print("J_phi2chi:\n", J_phi2chi)

examples/derive_planar_pcs.py

@@ -12,5 +12,7 @@
         / f"planar_pcs_ns-{NUM_SEGMENTS}.dill"
     )
     symbolically_derive_planar_pcs_model(
-        num_segments=NUM_SEGMENTS, filepath=sym_exp_filepath, simplify_expressions=True
+        num_segments=NUM_SEGMENTS,
+        filepath=sym_exp_filepath,
+        simplify_expressions=True if NUM_SEGMENTS < 3 else False,
     )

examples/simulate_planar_pcs.py

@@ -43,7 +43,7 @@
 strain_selector = jnp.array([True, False, False])
 
 # define initial configuration
-q0 = jnp.array([10 * jnp.pi])
+q0 = jnp.array([5 * jnp.pi])
 # number of generalized coordinates
 n_q = q0.shape[0]
 
@@ -95,8 +95,8 @@ def draw_robot(
 
 
 if __name__ == "__main__":
-    strain_basis, forward_kinematics_fn, dynamical_matrices_fn, auxiliary_fns = planar_pcs.factory(
-        sym_exp_filepath, strain_selector
+    strain_basis, forward_kinematics_fn, dynamical_matrices_fn, auxiliary_fns = (
+        planar_pcs.factory(sym_exp_filepath, strain_selector)
     )
     batched_forward_kinematics = vmap(
         forward_kinematics_fn, in_axes=(None, None, 0), out_axes=-1
@@ -142,9 +142,38 @@ def draw_robot(
     # the evolution of the generalized velocities
     q_d_ts = sol.ys[:, n_q:]
 
+    # evaluate the forward kinematics along the trajectory
+    chi_ee_ts = vmap(forward_kinematics_fn, in_axes=(None, 0, None))(
+        params, q_ts, jnp.array([jnp.sum(params["l"])])
+    )
+    # plot the end-effector position along the trajectory
+    plt.figure()
+    plt.plot(chi_ee_ts[0, :], chi_ee_ts[1, :])
+    plt.axis("equal")
+    plt.grid(True)
+    plt.xlabel("End-effector x [m]")
+    plt.ylabel("End-effector y [m]")
+    plt.tight_layout()
+    plt.show()
+    # plot end-effector position vs time
+    plt.figure()
+    plt.plot(video_ts, chi_ee_ts[:, 0], label="x")
+    plt.plot(video_ts, chi_ee_ts[:, 1], label="y")
+    plt.xlabel("Time [s]")
+    plt.ylabel("End-effector Position [m]")
+    plt.legend()
+    plt.grid(True)
+    plt.box(True)
+    plt.tight_layout()
+    plt.show()
+
     # plot the energy along the trajectory
-    kinetic_energy_fn_vmapped = vmap(partial(auxiliary_fns["kinetic_energy_fn"], params))
-    potential_energy_fn_vmapped = vmap(partial(auxiliary_fns["potential_energy_fn"], params))
+    kinetic_energy_fn_vmapped = vmap(
+        partial(auxiliary_fns["kinetic_energy_fn"], params)
+    )
+    potential_energy_fn_vmapped = vmap(
+        partial(auxiliary_fns["potential_energy_fn"], params)
+    )
     U_ts = potential_energy_fn_vmapped(q_ts)
     T_ts = kinetic_energy_fn_vmapped(q_ts, q_d_ts)
     plt.figure()
@@ -155,6 +184,7 @@ def draw_robot(
     plt.legend()
     plt.grid(True)
     plt.box(True)
+    plt.tight_layout()
     plt.show()
 
     # create video

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.10"  # Required
+version = "0.0.11"  # Required
 
 # This is a one-line description or tagline of what your project does. This
 # corresponds to the "Summary" metadata field:
@@ -88,10 +88,10 @@ classifiers = [  # Optional
   # that you indicate you support Python 3. These classifiers are *not*
   # checked by "pip install". See instead "python_requires" below.
     "Programming Language :: Python :: 3",
-    "Programming Language :: Python :: 3.9",
     "Programming Language :: Python :: 3.10",
     "Programming Language :: Python :: 3.11",
     "Programming Language :: Python :: 3.12",
+    "Programming Language :: Python :: 3.13",
     "Programming Language :: Python :: 3 :: Only",
 ]
 

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)