v0.0.1 Initial package structure

Author: Danfoa

README.md

@@ -0,0 +1,83 @@
+# Spring-loaded Inverted Pendulum (SLIP) Control
+
+This repository intends to provide a base for the implementation, visualization, and *open-access* sharing of
+SLIP control for research and education. The SLIP model is a bio-inspired low-order template model for dynamics of
+(high-speed) legged locomotion.  <cite>[[1]][1]</cite> It has been proven valuable in modeling the sagittal dynamics of the Center of Mass (CoM)
+of animals at high-speed locomotion, and extensions of this model were to model 3D bipedal walking and running.
+
+![slip_repo_into](https://user-images.githubusercontent.com/8356912/135597417-ea86cddf-d9bd-4dc6-8f34-6a32abb94521.png)
+
+https://user-images.githubusercontent.com/8356912/135605031-1d53dafc-bf50-4a66-b331-90fb9c892caf.mp4
+
+*Motivation: There is a lot of research and educational content based on the SLIP model but no shared code or initiative
+that allows playing with the model control without implementing everything from scratch. The ideal scenario is for this repo to become a mechanism for people to contribute different approaches to SLIP control, visualization, debugging,
+and educational tools.*
+
+### Main Features
+The repository offers python class objects with several utility functions to create SLIP models and plan passive or controlled trajectories, plus visualization tools for these trajectories. The main classes are:
+
+- `SlipModel`: a python class representation of a SLIP model. The class enables the generation of passive
+  trajectories by integration of the hybrid-dynamics differential equations governing SLIP, and some additional utility
+  functions for visualization and state handling.
+
+- `SlipGaitCycle` and `SlipGaitCycleCtrl`: python classes representing a full gait cycle (flight and stance phases) of
+  a SLIP model. The class provides an intuitive interface to storing the cartesian and polar trajectories of the SLIP
+  gait cycle model, along with accessors to full-cycle cartesian trajectories, foot contact positions, touch-down/take-off leg angles, and more. The controlled version is intended to store both the passive and controlled trajectories of the model.
+- `SlipTrajectory`: a class containing a sequence of consecutive `SlipGaitCycle`s, ensuring continuity in the trajectory. This class helps in saving and restoring a trajectory, plotting, and more.
+
+- `SlipTrajectoryTree`: a tree data structure intended to be used to plan multi-cycle trajectories.
+  It offers the capability to store multiple possible trajectories emerging from different future touch down angles,
+  selection of the most optimal and some plotting utilities.
+
+- Plotting: both for control development, debugging and especially for education, visual aid of the model dynamics and response. In utils, you will find plotting and (soon) animating utility functions to generate rich visualizations of
+  `SlipTrajectory`s and `SlipTrajectoryTree`s.
+
+The structure and functions of the classes are based on what I found useful when developing a multi-cycle
+trajectory planner for SLIP. Any modifications and enhancements are more than welcome. Submit a PR! :).
+
+### Controllers
+
+In the `slip_control/controllers` you will find single-cycle and multi-cycle trajectory control controllers. There
+is only one controller so far, but I encourage you to contribute many:
+
+- [x] Differentially Flat optimal control of SLIP: A controller of an extended version of the SLIP model using hip torque and leg force actuation. Details in [[3]] (Chapter 4.1). This controller is an adapted version of [[2]].
+
+### Instalation
+
+This repo is still in early development therefore, there is no release, but installation is easy through pip via:
+
+```buildoutcfg
+# Clone repo 
+git clone https://github.com/Danfoa/slip_control.git
+# Move to repo folder 
+cd slip_control 
+# Use pip to install repo and dependencies
+pip install -e .
+```
+
+#### Examples
+
+A preliminary example of the visualization tools and the Differentially Flat Controller can be run by 
+
+```buildoutcfg
+cd <repo>
+# Run example
+python examples/diff_flat_control_and_visualization.py 
+```
+
+
+### References and recommended literature
+
+By far, the best summary of the SLIP model and its control is [[1]]. For the model, biomechanical foundations [[4]] offers a
+a great introduction to the value of SLIP as a generic locomotion model.
+
+- [[1]] Geyer, H., et al. "Gait based on the spring-loaded inverted pendulum." Humanoid Robotics: A Reference. Springer Netherlands, 2018. 1-25.
+- [[2]] Chen, Hua, Patrick M. Wensing, and Wei Zhang. "Optimal control of a differentially flat two-dimensional spring-loaded inverted pendulum model." IEEE Robotics and Automation Letters 5.2 (2019): 307-314.
+- [[3]] Ordoñez Apraez, Daniel Felipe. Learning to run naturally: guiding policies with the Spring-Loaded Inverted Pendulum. MS thesis. Universitat Politècnica de Catalunya, 2021.
+- [[4]] Full, Robert J., Claire T. Farley, and Jack M. Winters. "Musculoskeletal dynamics in rhythmic systems: a comparative approach to legged locomotion." Biomechanics and neural control of posture and movement. Springer, New York, NY, 2000. 192-205.
+
+
+[1]: https://link.springer.com/referenceworkentry/10.1007%2F978-94-007-7194-9_43-1#:~:text=The%20spring%2Dloaded%20inverted%20pendulum%20(SLIP)%20describes%20gait%20with,control%20of%20compliant%20legged%20locomotion. "Hello"
+[2]: https://ieeexplore.ieee.org/abstract/document/8917684?casa_token=u34S9aFjkVAAAAAA:JqTD1t2tg4w_Xojfzc18fPggjZGO3mztxcMUzrY-pzuCeqwhvxjL5Dz7lHbFgkgOwUVzu6YZsEE
+[3]: https://upcommons.upc.edu/handle/2117/348196
+[4]: https://link.springer.com/chapter/10.1007/978-1-4612-2104-3_13

examples/diff_flat_control_and_visualization.py

@@ -0,0 +1,101 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+# @Time    : 1/10/21
+# @Author  : Daniel Ordonez 
+# @email   : daniels.ordonez@gmail.com
+from math import pi as PI
+import numpy as np
+import matplotlib.pyplot as plt
+from argparse import ArgumentParser
+
+from slip_control.slip.slip_trajectory import SlipTrajectory
+from slip_control.slip.slip_model import SlipModel, X, X_DOT, X_DDOT, Z, Z_DOT, Z_DDOT
+from slip_control.controllers.target_to_states_generator import CycleStateGenerator, ForwardSpeedStateGenerator
+from slip_control.controllers.diff_flat_slip_controller import SlipDiffFlatController
+from slip_control.utils import plot_utils
+
+cmap = plt.cm.get_cmap('gist_heat')
+
+if __name__ == "__main__":
+
+    # Instantiate SLIP model
+    m = 80  # [kg]
+    r0 = 1.0  # [m]
+    n_legs = 1
+    k_rel = 10.7
+    slip = SlipModel(mass=m, leg_length=r0, k_rel=k_rel * n_legs)
+
+    g = SlipModel.g
+
+    # Error deviation weights during the stance trajectory
+    traj_weights = np.array([1., 1., 1., 1., 1., 1.])
+    traj_weights /= np.linalg.norm(traj_weights)
+    # Error deviation weights of target take-off states
+    take_off_state_error_weights = np.array([0.0, 1.0, 0., 1.0, 1.0, 0.])
+    take_off_state_error_weights /= np.linalg.norm(take_off_state_error_weights)
+
+
+    n_cycles = 5 # Generate a trajectory of 5 cycles
+    max_theta_dot = 4*PI # [rad/s] max angular leg velocity during flight
+    # Define a forward velocity
+    forward_speed = 4 * slip.r0  # [m/s]
+    # Define a desired gait duty cycle (time of stance / time of cycle) in [0.2, 1.0]
+    duty_cycle = 0.8
+
+    z_init = slip.r0
+    # Set an initial state (assumed to be a flight phase state)  [x, x', x'', z, z', z'']
+    init_to_state = np.array([0.0, forward_speed, 0.0, z_init, 0.0, -g])
+    # Set a desired take off state defining the forward and vertical velocity desired
+    to_des_state = init_to_state
+
+    # Configure Differentially flat controller
+    slip_controller = SlipDiffFlatController(slip_model=slip,
+                                             traj_weights=traj_weights,
+                                             max_flight_theta_dot=max_theta_dot,
+                                             debug=False)
+    to_state_generator = ForwardSpeedStateGenerator(slip_model=slip, target_state_weights=take_off_state_error_weights,
+                                                    desired_forward_speed=forward_speed,
+                                                    desired_duty_cycle=duty_cycle)
+    slip_controller.target_to_state_generator = to_state_generator
+
+    # Generate SLIP trajectory tree without future cycle planning
+    tree = slip_controller.generate_slip_trajectory_tree(desired_gait_cycles=n_cycles,
+                                                         initial_state=init_to_state,
+                                                         max_samples_per_cycle=30,
+                                                         angle_epsilon=np.deg2rad(.02),
+                                                         look_ahead_cycles=0)
+    slip_traj_no_future = tree.get_optimal_trajectory()
+    plot_utils.plot_slip_trajectory(slip_traj_no_future, plot_passive=True, plot_td_angles=True,
+                                          title="Without future cycle planning",
+                                          color=(23/255., 0/255., 194/255.))
+    plt.show()
+
+    # Generate SLIP trajectory tree with future cycle planning
+    tree = slip_controller.generate_slip_trajectory_tree(desired_gait_cycles=n_cycles,
+                                                         initial_state=init_to_state,
+                                                         max_samples_per_cycle=30,
+                                                         angle_epsilon=np.deg2rad(.02),
+                                                         look_ahead_cycles=1)
+    slip_traj = tree.get_optimal_trajectory()
+    plot_utils.plot_slip_trajectory(slip_traj, plot_passive=True, plot_td_angles=True,
+                                          title="With future cycle planing",
+                                          color=(23 / 255., 154 / 255., 194 / 255.))
+    plt.show()
+
+    # Plot controlled trajectory tree
+    print("This takes a while... should optimize soon")
+    tree.plot()
+    plt.show()
+
+    # Compare first two cycles.
+    short_traj = SlipTrajectory(slip, slip_gait_cycles=slip_traj.gait_cycles[:2])
+    short_traj_no_future = SlipTrajectory(slip, slip_gait_cycles=slip_traj_no_future.gait_cycles[:2])
+    axs = plot_utils.plot_slip_trajectory(short_traj, plot_passive=True, plot_td_angles=True,
+                                          color=(23 / 255., 154 / 255., 194 / 255.))
+    axs = plot_utils.plot_slip_trajectory(short_traj_no_future, plot_passive=True, plot_td_angles=True, plt_axs=axs,
+                                          color=(23/255., 0/255., 194/255.))
+    plt.show()
+
+    # Plot limit cycles of controlled trajectory
+    phase_axs = plot_utils.plot_limit_cycles(slip_traj)
+    plt.show()

setup.py

@@ -0,0 +1,29 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+# @Time    : 1/10/21
+# @Author  : Daniel Ordoñez
+# @email   : daniels.ordonez@gmail.com
+
+from distutils.core import setup
+
+with open("README.md", "r", encoding="utf-8") as fh:
+    long_description = fh.read()
+
+setup(name='slip_control',
+      version='0.0.1',
+      description='Spring Loaded Inverted Pendulum control and visualization python tools',
+      long_description=long_description,
+      long_description_content_type="text/markdown",
+      author='Daniel Ordonez',
+      author_email='daniel.ordonez@gmail.com',
+      url='https://github.com/Danfoa/slip_control',
+      packages=['slip', 'controllers', 'utils'],
+      package_dir={'': "slip_control"},
+      classifiers=[
+          "Programming Language :: Python :: 3",
+          "License :: OSI Approved :: MIT License",
+          "Operating System :: OS Independent",
+      ],
+      requires=['numpy', 'scipy', 'matplotlib', 'tqdm', 'cvxpy'],
+      python_requires=">=3.6",
+      )

slip_control/controllers/diff_flat_slip_controller.py

@@ -57,6 +57,17 @@ def __init__(self, slip_model: SlipModel, traj_weights,
     def generate_slip_trajectory_tree(self, initial_state, desired_gait_cycles=np.inf, desired_duration=np.inf,
                                       look_ahead_cycles=0, max_samples_per_cycle=20, angle_epsilon=np.deg2rad(1),
                                       verbose=True) -> TrajectoryTreeNode:
+        """
+        Generate a trajectory tree by testing and controlling multiple touch down angles per gait cycle
+        :param initial_state: Initial cartesian state assumed to be part of a flight phase
+        :param desired_gait_cycles: Number of gait cycles to plan for
+        :param desired_duration: Maximum time of a trajectory to be returned
+        :param look_ahead_cycles: Number of gait cycles in the future to look for when planning.
+        :param max_samples_per_cycle: Maximum number of touch down angles evaluated per gait cycle
+        :param angle_epsilon: Minimum difference in touch down angle to consider optimal
+        :param verbose:
+        :return: A trajectory tree with all the evaluated trajectories.
+        """
         assert xor(np.isinf(desired_gait_cycles), np.isinf(
             desired_duration)), 'Provide ONE stopping criteria: `desired_gait_cycles` or `desired_duration`'
         opt_start = time.time()

slip_control/slip/slip_gait_cycle.py

@@ -160,25 +160,25 @@ def __init__(self, slip_model, t_flight, flight_cartesian_traj, t_stance, stance
     @property
     def stance_cartesian_traj(self) -> ndarray:
         if self._stance_cartesian_traj is None:
-            self._stance_cartesian_traj = self.slip_model.polar_to_cartesian(trajectory=(self.stance_polar_traj),
-                                                                             control_signal=(self.control_signal),
-                                                                             foot_contact_pos=(self.foot_contact_pos))
+            self._stance_cartesian_traj = self.slip_model.polar_to_cartesian(trajectory=self.stance_polar_traj,
+                                                                             control_signal=self.control_signal,
+                                                                             foot_contact_pos=self.foot_contact_pos)
         return self._stance_cartesian_traj
 
     @property
     def take_off_state(self) -> ndarray:
         if self._stance_cartesian_traj is None:
-            return self.slip_model.polar_to_cartesian((self.stance_polar_traj[:, -1]),
-                                                      control_signal=(self.control_signal[:, -1]),
-                                                      foot_contact_pos=(self.foot_contact_pos))
+            return self.slip_model.polar_to_cartesian(self.stance_polar_traj[:, -1],
+                                                      control_signal=self.control_signal[:, -1],
+                                                      foot_contact_pos=self.foot_contact_pos)
         else:
             return np.array(self.stance_cartesian_traj[:, -1])
 
     @property
     def touch_down_state(self):
         if self._stance_cartesian_traj is None:
-            return self.slip_model.polar_to_cartesian((self.stance_polar_traj[:, 0]),
-                                                      control_signal=(self.control_signal[:, 0]),
-                                                      foot_contact_pos=(self.foot_contact_pos))
+            return self.slip_model.polar_to_cartesian(self.stance_polar_traj[:, 0],
+                                                      control_signal=self.control_signal[:, 0],
+                                                      foot_contact_pos=self.foot_contact_pos)
         else:
             return np.array(self.stance_cartesian_traj[:, 0])

slip_control/slip/slip_model.py

@@ -156,17 +156,15 @@ def polar_to_cartesian(self, trajectory, control_signal=None, foot_contact_pos=0
         assert polar_traj.shape[0] == 4, 'Provide a valid (4, k) polar trajectory: %s' % polar_traj.shape
         if polar_traj.ndim == 1:
             polar_traj = np.expand_dims(polar_traj, axis=1)
-            u_ctrl = control_signal
+
+        if control_signal is None:
+            u_ctrl = np.zeros((2, polar_traj.shape[-1]))
         else:
-            if control_signal is None:
-                u_ctrl = np.zeros((2, polar_traj.shape[(-1)]))
-            else:
-                u_ctrl = np.array(control_signal)
-            if u_ctrl.ndim == 1:
-                u_ctrl = np.expand_dims(u_ctrl, axis=1)
-            assert u_ctrl.shape[0] == 2, 'Provide a valid (2, k) control input vector'
-            assert polar_traj.shape[1] == u_ctrl.shape[1], 'Len of trajectory: polar = %d | control_input = %d' % (
-                polar_traj.shape[1], u_ctrl.shape[1])
+            u_ctrl = np.expand_dims(control_signal, axis=1) if control_signal.ndim == 1 else np.array(control_signal)
+
+        assert u_ctrl.shape[0] == 2, 'Provide a valid (2, k) control input vector'
+        assert polar_traj.shape[1] == u_ctrl.shape[1], 'Len of trajectory: polar = %d | control_input = %d' % \
+                                                       (polar_traj.shape[1], u_ctrl.shape[1])
         theta = polar_traj[0, :]
         theta_dot = polar_traj[1, :]
         r = polar_traj[2, :]

slip_control/slip/slip_trajectory.py

@@ -138,9 +138,9 @@ def gen_continuous_gait_phase_signal(self) -> interp1d:
             touch_down_time = cycle.t_flight[(-1)]
             t.extend([cycle.start_time, touch_down_time, cycle.end_time - dt])
 
-        t.append(self.end_time)
+        # t.append(self.end_time)
         gait_cycle_phase = np.linspace(0, 2 * np.pi, 3)
-        period_cycle_phase = np.concatenate([gait_cycle_phase] * len(self) + gait_cycle_phase[(-1)])
+        period_cycle_phase = np.concatenate([gait_cycle_phase] * len(self))
         continuous_gait_phase_signal = interp1d(x=t, y=period_cycle_phase, kind='linear', fill_value='extrapolate',
                                                 assume_sorted=True)
         return continuous_gait_phase_signal

slip_control/slip/slip_trajectory_tree.py

@@ -71,7 +71,7 @@ def optimal_td_angle(self) -> Optional[float]:
         td_angles = list(self.child_nodes.keys())
         costs = [node.compute_opt_cost() for node in self.child_nodes.values()]
         if np.all(np.isnan(costs)):
-            self._optimal_td_angle = td_angles[(-1)]
+            best_branch_idx = 0
         else:
             best_branch_idx = np.nanargmin(costs)
         self._optimal_td_angle = td_angles[best_branch_idx]
@@ -148,4 +148,3 @@ def plot(self, optimal_color=(0, 0, 0, 1), sub_optimal_color=(1, 0, 0, 0.15), un
             outer_pbar.update()
 
         outer_pbar.close()
-        return plt_axs.get_figure()

slip_control/utils/plot_utils.py

@@ -57,7 +57,7 @@ def plot_cartesian_trajectory(traj, t, target_state=None, foot_pos=None, touch_d
 
             if touch_down_angle is not None:
                 xz_ax.text(foot_pos + 0.01, 0.05, '%.1f°' % touch_down_angle)
-        xz_ax.grid('on', alpha=0.3)
+        xz_ax.grid('on', alpha=0.15)
         xz_ax.set_ylim(bottom=0.0, auto=True)
         xz_ax.set_xlabel('$\\mathbf{y[m]}$', fontweight='bold', fontsize=LABEL_FONT_SIZE)
         xz_ax.set_ylabel('$\\mathbf{z[m]}$', fontweight='bold', fontsize=LABEL_FONT_SIZE)
@@ -71,7 +71,7 @@ def plot_cartesian_trajectory(traj, t, target_state=None, foot_pos=None, touch_d
         ax.set_ylabel((x_titles[i]), fontweight='bold', fontsize=LABEL_FONT_SIZE)
         if i == len(x_axs) - 1:
             ax.set_xlabel('t[s]', fontweight='bold', fontsize=LABEL_FONT_SIZE)
-        ax.grid('on', alpha=0.3)
+        ax.grid('on', alpha=0.15)
         ax.set_xlim(xmin=0.0, auto=True)
 
     z_titles = ['$\\mathbf{z [m]}$', '$\\mathbf{\\dot{z} [m/s]}$', '$\\mathbf{\\ddot{z} [m/s^2]}$']
@@ -82,7 +82,7 @@ def plot_cartesian_trajectory(traj, t, target_state=None, foot_pos=None, touch_d
         ax.set_ylabel((z_titles[i]), fontweight='bold', fontsize=LABEL_FONT_SIZE)
         if i == len(z_axs) - 1:
             ax.set_xlabel('t[s]', fontweight='bold', fontsize=LABEL_FONT_SIZE)
-        ax.grid('on', alpha=0.3)
+        ax.grid('on', alpha=0.15)
         ax.set_xlim(xmin=0.0, auto=True)
 
     return (fig, plt_axs)
@@ -109,15 +109,15 @@ def plot_polar_trajectory(polar_traj, t, target_state=None, axs=None, color='k',
         ax.plot((t[0]), (polar_traj[(i + 2, 0)]), color=color, marker='o', markersize='6', markeredgecolor='k')
         ax.plot((t[(-1)]), (target_state[(i + 2)]), 'D', color=target_color, markersize='10')
         ax.set_ylabel((y_titles[i]), rotation=0)
-        ax.grid('on', alpha=0.3)
+        ax.grid('on', alpha=0.15)
 
     z_titles = ['$\\theta$', '$\\dot{\\theta}$', '$\\ddot{\\theta}$']
     for i, ax in enumerate(theta_axs.flatten()):
         ax.plot(t, (np.rad2deg(polar_traj[i, :])), 'k-', markersize='2', label=label, color=color)
         ax.plot((t[0]), (np.rad2deg(polar_traj[(i, 0)])), color=color, marker='o', markersize='6', markeredgecolor='k')
         ax.plot((t[(-1)]), (np.rad2deg(target_state[i])), 'D', color=target_color, markersize='10')
         ax.set_ylabel((z_titles[i]), rotation=0)
-        ax.grid('on', alpha=0.3)
+        ax.grid('on', alpha=0.15)
 
     return (
         fig, axs)
@@ -141,7 +141,7 @@ def plot_control_trajectory(u_traj, t, theta, r, r0, k, axs=None, color='k', lab
     ax.plot(t, displacement, 'k-o', label=label, color=color, markersize=marker_size, linestyle=linestyle,
             marker=marker)
     ax.set_ylabel('Displacement [m]')
-    ax.grid('on', alpha=0.3)
+    ax.grid('on', alpha=0.15)
     ax = extension_ax[1]
     ax.set_title('Leg length displacement $u_{1}$')
     resultant_force = k * (r0 - r + u_traj[0, :])
@@ -155,13 +155,13 @@ def plot_control_trajectory(u_traj, t, theta, r, r0, k, axs=None, color='k', lab
     ax.fill_between((theta * 180 / np.pi), resultant_force, passive_force, alpha=0.1, color=color)
     ax.set_ylabel('Radial Force [m]')
     ax.set_xlabel('Hip angle $\\theta$ [Deg]')
-    ax.grid('on', alpha=0.3)
+    ax.grid('on', alpha=0.15)
     ax = torque_ax[0]
     ax.set_title('Hip torque $u_{2}$')
     ax.plot(t, hip_torque, 'k-o', label=label, color=color, markersize=marker_size, linestyle=linestyle, marker=marker)
     ax.set_ylabel('Torque [Nm]')
     ax.set_xlabel('Time [s]')
-    ax.grid('on', alpha=0.3)
+    ax.grid('on', alpha=0.15)
     ax = torque_ax[1]
     ax.set_title('Hip torque $u_{2}$ vs $\\theta$')
     ax.plot((theta * 180 / np.pi), hip_torque, 'k-o', label=label, color=color, markersize=marker_size,
@@ -170,7 +170,7 @@ def plot_control_trajectory(u_traj, t, theta, r, r0, k, axs=None, color='k', lab
     ax.fill_between((theta * 180 / np.pi), hip_torque, alpha=0.1, color=color)
     ax.set_ylabel('Torque [Nm]')
     ax.set_xlabel('Hip angle $\\theta$ [Deg]')
-    ax.grid('on', alpha=0.3)
+    ax.grid('on', alpha=0.15)
     ax.legend(bbox_to_anchor=(1.05, 1), loc='upper left')
     return (
         fig, axs)
@@ -229,96 +229,75 @@ def plot_slip_trajectory(slip_traj: Union[(SlipTrajectory, SlipGaitCycleCtrl)],
     return plt_axs
 
 
-def plot_limit_cycles(slip_traj: SlipTrajectory, start_cycle=0, axs=None, cmap_name='copper'):
-    print(('Plotting slip_traj limit gait_cycle for %d cycles...' % slip_traj.num_cycles), end='')
-    fig_size = (15, 10)
+def plot_limit_cycles(slip_traj: SlipTrajectory, axs=None, cmap_name='copper', fig_size=(15, 10)):
     LABEL_FONT_SIZE = 12
     cmap = plt.cm.get_cmap(cmap_name)
-    if axs is None:
-        fig, axs = plt.subplots(4, 3, figsize=fig_size)
 
     def plot_limit_cycle_metric(x_flight, y_flight, x_stance, y_stance, x_name, y_name, flight_phase, stance_phase, ax,
                                 ax2, ax3, color):
         if x_flight is not None:
             ax.plot(x_flight, y_flight, label=label, color=color, markersize=marker_size, linestyle='dotted')
-        else:
-            ax.plot(x_stance, y_stance, label=label, color=color, markersize=marker_size, linestyle='-')
-            ax.plot((x_stance[0]), (y_stance[0]), 'D', color=color, markersize='6', markeredgecolor='k')
-            ax.plot((x_stance[(-1)]), (y_stance[(-1)]), 'X', color=color, markersize='6', markeredgecolor='k')
-            ax.set_xlabel(x_name, fontweight='bold', fontsize=LABEL_FONT_SIZE)
-            ax.set_ylabel(y_name, fontweight='bold', fontsize=LABEL_FONT_SIZE)
-            ax.grid('on', alpha=0.3)
-            if x_flight is not None:
-                ax2.plot(flight_phase, y_flight, label=label, color=color, markersize=marker_size, linestyle='dotted')
-            ax2.plot((stance_phase[0]), (y_stance[0]), 'D', color=color, markersize='6', markeredgecolor='k')
-            ax2.plot((stance_phase[(-1)]), (y_stance[(-1)]), 'X', color=color, markersize='6', markeredgecolor='k')
-            ax2.plot(stance_phase, y_stance, label=label, color=color, markersize=marker_size, linestyle='-')
-            ax2.set_ylabel(y_name, fontweight='bold', fontsize=LABEL_FONT_SIZE)
-            ax2.set_xlabel('$\\phi$', fontweight='bold', fontsize=LABEL_FONT_SIZE)
-            ax2.grid('on', alpha=0.8)
-            if x_flight is not None:
-                ax3.plot(flight_phase, x_flight, label=label, color=color, markersize=marker_size, linestyle='dotted')
-        ax3.plot((stance_phase[0]), (x_stance[0]), 'D', color=color, markersize='6', markeredgecolor='k')
-        ax3.plot((stance_phase[(-1)]), (x_stance[(-1)]), 'X', color=color, markersize='6', markeredgecolor='k')
+        ax.plot(x_stance, y_stance, label=label, color=color, markersize=marker_size, linestyle='-')
+        ax.plot((x_stance[0]), (y_stance[0]), 'D', color=color, markersize='4', markeredgecolor='k')
+        ax.plot((x_stance[(-1)]), (y_stance[(-1)]), 'X', color=color, markersize='4', markeredgecolor='k')
+        ax.set_xlabel(x_name, fontweight='bold', fontsize=LABEL_FONT_SIZE)
+        ax.set_ylabel(y_name, fontweight='bold', fontsize=LABEL_FONT_SIZE)
+        ax.grid('on', alpha=0.15)
+        if x_flight is not None:
+            ax2.plot(flight_phase, y_flight, label=label, color=color, markersize=marker_size, linestyle='dotted')
+        ax2.plot(stance_phase, y_stance, label=label, color=color, markersize=marker_size, linestyle='-')
+        ax2.plot((stance_phase[0]), (y_stance[0]), 'D', color=color, markersize='4', markeredgecolor='k')
+        ax2.plot((stance_phase[(-1)]), (y_stance[(-1)]), 'X', color=color, markersize='4', markeredgecolor='k')
+        ax2.set_ylabel(y_name, fontweight='bold', fontsize=LABEL_FONT_SIZE)
+        ax2.set_xlabel('$\\phi$', fontweight='bold', fontsize=LABEL_FONT_SIZE)
+        ax2.grid('on', alpha=0.15)
+        if x_flight is not None:
+            ax3.plot(flight_phase, x_flight, label=label, color=color, markersize=marker_size, linestyle='dotted')
         ax3.plot(stance_phase, x_stance, label=label, color=color, markersize=marker_size, linestyle='-')
+        ax3.plot((stance_phase[0]), (x_stance[0]), 'D', color=color, markersize='4', markeredgecolor='k')
+        ax3.plot((stance_phase[(-1)]), (x_stance[(-1)]), 'X', color=color, markersize='4', markeredgecolor='k')
         ax3.set_ylabel(x_name, fontweight='bold', fontsize=LABEL_FONT_SIZE)
         ax3.set_xlabel('$\\phi$', fontweight='bold', fontsize=LABEL_FONT_SIZE)
-        ax3.grid('on', alpha=0.8)
+        ax3.grid('on', alpha=0.15)
+
+    if axs is None:
+        fig, axs = plt.subplots(4, 3, figsize=fig_size)
 
-    for cycle in range(start_cycle, slip_traj.num_cycles):
-        color = cmap(cycle / slip_traj.num_cycles)
+    for i, cycle in enumerate(slip_traj.gait_cycles):
+        color = cmap(i / len(slip_traj))
         marker_size = 1
         label = None
-        flight_traj = slip_traj.flight_traj[cycle]
-        stance_traj = slip_traj.stance_traj[cycle]
-        stance_traj_polar = slip_traj.stance_polar_traj[cycle]
-        flight_phase = slip_traj.get_gait_phase(
-            np.linspace(start=(slip_traj.flight_times[cycle][0]), stop=(slip_traj.flight_times[cycle][(-1)]),
-                        num=(len(flight_traj[Z_DOT]))))
-        stance_phase = slip_traj.get_gait_phase(
-            np.linspace(start=(slip_traj.stance_times[cycle][0]), stop=(slip_traj.stance_times[cycle][(-2)]),
-                        num=(len(stance_traj[Z_DOT]))))
+        flight_traj = cycle.flight_cartesian_traj
+        stance_traj = cycle.stance_cartesian_traj
+        stance_traj_polar = cycle.stance_polar_traj
+
+        flight_phase = slip_traj.get_gait_phase(np.linspace(start=cycle.t_flight[0], stop=cycle.t_flight[-1],
+                                                            num=len(cycle.t_flight)))
+        stance_phase = slip_traj.get_gait_phase(np.linspace(start=cycle.t_stance[0], stop=cycle.t_stance[-2],
+                                                            num=len(cycle.t_stance)))
+
         plot_limit_cycle_metric(x_flight=(flight_traj[X_DOT]), y_flight=(flight_traj[Z_DOT]),
-                                x_stance=(stance_traj[X_DOT]),
-                                y_stance=(stance_traj[Z_DOT]),
-                                x_name='$\\dot{y}[m/s]}$',
-                                y_name='$\\dot{z}[m/s]}$',
-                                flight_phase=flight_phase,
-                                stance_phase=stance_phase,
-                                ax=(axs[(0, 0)]),
-                                ax2=(axs[(0, 1)]),
-                                ax3=(axs[(0, 2)]),
-                                color=color)
+                                x_stance=(stance_traj[X_DOT]), y_stance=(stance_traj[Z_DOT]),
+                                x_name='$\\dot{x}[m/s]}$', y_name='$\\dot{z}[m/s]}$',
+                                flight_phase=flight_phase, stance_phase=stance_phase,
+                                ax=(axs[(0, 0)]), ax2=(axs[(0, 1)]), ax3=(axs[(0, 2)]), color=color)
         plot_limit_cycle_metric(x_flight=(flight_traj[X_DDOT]), y_flight=(flight_traj[Z_DDOT]),
-                                x_stance=(stance_traj[X_DDOT]),
-                                y_stance=(stance_traj[Z_DDOT]),
-                                x_name='$\\ddot{y}[m/s]}$',
-                                y_name='$\\ddot{z}[m/s]}$',
-                                flight_phase=flight_phase,
-                                stance_phase=stance_phase,
-                                ax=(axs[(1, 0)]),
-                                ax2=(axs[(1, 1)]),
-                                ax3=(axs[(1, 2)]),
+                                x_stance=(stance_traj[X_DDOT]), y_stance=(stance_traj[Z_DDOT]),
+                                x_name='$\\ddot{x}[m/s]}$', y_name='$\\ddot{z}[m/s]}$',
+                                flight_phase=flight_phase, stance_phase=stance_phase,
+                                ax=(axs[(1, 0)]), ax2=(axs[(1, 1)]), ax3=(axs[(1, 2)]),
                                 color=color)
-        plot_limit_cycle_metric(x_flight=None, y_flight=None, x_stance=(np.rad2deg(stance_traj_polar[THETA])),
-                                y_stance=(stance_traj_polar[R]),
-                                x_name='$\\theta[deg]$',
-                                y_name='$r[m]$',
-                                flight_phase=flight_phase,
-                                stance_phase=stance_phase,
-                                ax=(axs[(2, 0)]),
-                                ax2=(axs[(2, 1)]),
-                                ax3=(axs[(2, 2)]),
+        plot_limit_cycle_metric(x_flight=None, y_flight=None,
+                                x_stance=(np.rad2deg(stance_traj_polar[THETA])), y_stance=(stance_traj_polar[R]),
+                                x_name='$\\theta[deg]$', y_name='$r[m]$',
+                                flight_phase=flight_phase, stance_phase=stance_phase,
+                                ax=(axs[(2, 0)]), ax2=(axs[(2, 1)]), ax3=(axs[(2, 2)]),
                                 color=color)
-        plot_limit_cycle_metric(x_flight=None, y_flight=None, x_stance=(np.rad2deg(stance_traj_polar[THETA_DOT])),
-                                y_stance=(stance_traj_polar[R_DOT]),
-                                x_name='$\\dot{\\theta}[deg/s]$',
-                                y_name='$\\dot{r}[m/s]$',
-                                flight_phase=flight_phase,
-                                stance_phase=stance_phase,
-                                ax=(axs[(3, 0)]),
-                                ax2=(axs[(3, 1)]),
-                                ax3=(axs[(3, 2)]),
+        plot_limit_cycle_metric(x_flight=None, y_flight=None,
+                                x_stance=(np.rad2deg(stance_traj_polar[THETA_DOT])), y_stance=(stance_traj_polar[R_DOT]),
+                                x_name='$\\dot{\\theta}[deg/s]$', y_name='$\\dot{r}[m/s]$',
+                                flight_phase=flight_phase, stance_phase=stance_phase,
+                                ax=(axs[(3, 0)]), ax2=(axs[(3, 1)]), ax3=(axs[(3, 2)]),
                                 color=color)
-
+    plt.tight_layout()
     return axs