{ "cells": [ { "cell_type": "markdown", "id": "9bd6dd9a-a0f6-4248-89d4-657c318e0d79", "metadata": {}, "source": [ "# Tutorial 03: The Animator" ] }, { "cell_type": "markdown", "id": "3c18bd9e-7470-4b09-b7a7-fac8bb2dec17", "metadata": {}, "source": [ "## Tutorial Description" ] }, { "cell_type": "markdown", "id": "b7e48aef-41be-4597-be37-1bd196bc97a5", "metadata": {}, "source": [ "This tutorial covers use the animator interface to create a real time plot of data during a simulation. We will demonstrate this by creating a \n", "phase-space plot of a forced pendulum." ] }, { "cell_type": "markdown", "id": "be64a904-72de-4411-8a77-37a630f0c3a4", "metadata": {}, "source": [ "## Imports" ] }, { "cell_type": "markdown", "id": "5ca5c838-ffae-45eb-9e60-0ad1e0ac8698", "metadata": {}, "source": [ "To begin, we import the same modules for the same reasons as tutorial 00." ] }, { "cell_type": "code", "execution_count": 1, "id": "b2b6dc1e-8420-4242-91a3-e73422ce713e", "metadata": {}, "outputs": [], "source": [ "from condynsate import Simulator as con_sim\n", "from condynsate import __assets__ as assets" ] }, { "cell_type": "markdown", "id": "dea83d70-5877-4057-b588-983d28579958", "metadata": {}, "source": [ "## Building the Project Class" ] }, { "cell_type": "markdown", "id": "507fb740-3a04-4ff7-af5e-fbbd3c73f962", "metadata": {}, "source": [ "We now create a `Project` class with `__init__` and `run` functions. In `__init__` a pendulum is loaded using the same technique as tutorial 02. Additionally, the animator is set up the plot the phase diagram of the pendulum while the simulation is running. In `run`, we cover how send state data to the animator." ] }, { "cell_type": "code", "execution_count": 2, "id": "098a10b0-6cec-4676-8cd2-1ac6b47ad94b", "metadata": {}, "outputs": [], "source": [ "class Project():\n", " def __init__(self):\n", " '''\n", " ##################################################################\n", " This time we want to use the animator, so we set animation to \n", " True.\n", " ##################################################################\n", " '''\n", " # Create a instance of the simulator\n", " self.s = con_sim(animation = True,\n", " animation_fr = 24.0,\n", " keyboard = False)\n", " \n", " # Load the pendulum in the orientation we want\n", " self.pendulum = self.s.load_urdf(urdf_path = assets['pendulum'],\n", " position = [0., 0., 0.05],\n", " yaw = 1.571,\n", " wxyz_quaternion = [1., 0., 0., 0],\n", " fixed = True,\n", " update_vis = True)\n", "\n", " # Set the initial angle of the pendulum joint\n", " self.s.set_joint_position(urdf_obj = self.pendulum,\n", " joint_name = 'chassis_to_arm',\n", " position = 0.698,\n", " initial_cond = True,\n", " physics = False)\n", " \n", " '''\n", " ##################################################################\n", " Once our URDF is loaded and initial conditions are set, we move \n", " on to creating the animator window. \n", " condynsate.simulator.add_plot is how we tell the animator that \n", " we want to add a plot to our animation GUI. We may call this \n", " function as many times as we like and each time a new plot \n", " will be added to the animation GUI. The arguments to this \n", " function are as follows:\n", " n_artists : int, optional\n", " The number of artists that draw on the plot\n", " The default is 1.\n", " plot_type: either 'line' or 'bar', optional\n", " The type of plot. May either be 'line' or 'bar'. The default\n", " is 'line'.\n", " title : string, optional\n", " The title of the plot. Will be written above the plot when\n", " rendered. The default is None.\n", " x_label : string, optional\n", " The label to apply to the x axis. Will be written under the plot\n", " when rendered. The default is None.\n", " y_label : string, optional\n", " The label to apply to the y axis. Will be written to the left of\n", " the plot when rendered. The default is None.\n", " x_lim : [float, float], optional\n", " The limits to apply to the x axis of the plot. A value of None\n", " will apply automatically updating limits to the corresponding\n", " bound of the axis. For example [None, 10.] will fix the upper\n", " bound to exactly 10, but the lower bound will freely change to\n", " show all data.The default is [None, None].\n", " y_lim : [float, float], optional\n", " The limits to apply to the y axis of the plot. A value of None\n", " will apply automatically updating limits to the corresponding\n", " bound of the axis. For example [None, 10.] will fix the upper\n", " bound to exactly 10, but the lower bound will freely change to\n", " show all data.The default is [None, None].\n", " h_zero_line : boolean, optional\n", " A boolean flag that indicates whether a horizontal line will be\n", " drawn on the y=0 line. The default is false\n", " v_zero_line : boolean, optional\n", " A boolean flag that indicates whether a vertical line will be\n", " drawn on the x=0 line. The default is false\n", " color : matplotlib color string or tuple of color strings, optional\n", " The color each artist draws in. When tuple, must have length \n", " n_artists. The default is 'black'.\n", " label : string or tuple of strings, optional\n", " The label applied to each artist. For line charts, \n", " the labels are shown in a legend in the top right of the plot. For\n", " bar charts, the labels are shown on the y axis next to their \n", " corresponging bars. When tuple, must have length n_artists.\n", " When None, no labels are made. The default is None.\n", " line_width : float or tuple of floats, optional\n", " The line weigth each artist uses. For line plots, this is the\n", " width of the plotted line, for bar charts, this is the width of \n", " the border around each bar. When tuple, must have length n_artists.\n", " The default is 1.5.\n", " line_style : line style string or tuple of ls strings, optional\n", " The line style each artist uses. For line plots, this is the\n", " style of the plotted line, for bar charts, this argument is not\n", " used and therefore ignored. When tuple, must have length n_artists.\n", " The default is 'solid'.\n", " tail : int or tuple of ints optional\n", " Specifies how many data points are used to draw a line. Only the\n", " most recently added data points are kept. Any data points added\n", " more than tail data points ago are discarded and not plotted. Only\n", " valid for line plots. When tuple, must have length n_artists. A \n", " value less than or equal to 0 means that no data is ever discarded \n", " and all data points added to the animator will be drawn. \n", " The default is -1.\n", " \n", " The function returns:\n", " plot_id : int\n", " A integer identifier that is unique to the plot created. \n", " This allows future interaction with this plot (adding data\n", " points, etc.).\n", " artist_ids : tuple of ints, optional\n", " A tuple of integer identifiers that are unique to the artist\n", " created. This allows future interaction with these artists (adding\n", " data points, etc.). Is only returned when n_artists > 1.\n", " \n", " plot_id can be considered a pointer to a specific \n", " plot. If you want to make edits to a plot, you must first \n", " identify which plot you are modifying. This is done with \n", " plot_id. Each subplot can have multiple artists. You can \n", " think of an artist as a pen. If you want to draw two lines on a \n", " plot at the same time, you will need two pens, i.e. two \n", " artists. Each time you add a plot with more than 1 artists, you \n", " will receive a list of artist_ids. You can again think of this as \n", " a list of pointers to each artists in that plot. Note that if a \n", " plot is defined to have only 1 artist, no artist_ids are returned. \n", " This is because there is no need to disambiguate artists.\n", " ##################################################################\n", " '''\n", " # Make plot for phase space\n", " self.p = self.s.add_plot(n_artists = 1, # Only 1 artist means no artist_ids are returned\n", " plot_type = 'line',\n", " title = \"Phase Space\",\n", " x_label = \"Angle [Deg]\",\n", " y_label = \"Angle Rate [Deg / Sec]\",\n", " color = \"k\",\n", " line_width = 2.5,\n", " line_style = \"-\",\n", " x_lim = [-40.,40],\n", " y_lim = [-275.,275],\n", " h_zero_line = True,\n", " v_zero_line = True)\n", "\n", " '''\n", " ##################################################################\n", " Once we are done adding subplots to the animator, we open the \n", " animator GUI.\n", " ##################################################################\n", " '''\n", " # Open the animator GUI\n", " self.s.start_animator()\n", "\n", " def run(self, max_time=None):\n", " '''\n", " ##################################################################\n", " This run function does all the same basic functions as in \n", " tutorial 02 but with the added functionality of real time \n", " animation of the phase of the pendulum.\n", " ##################################################################\n", " '''\n", " # Reset the simulator.\n", " self.s.reset()\n", "\n", " # Run the simulation loop until done\n", " while(not self.s.is_done):\n", " # Get the pendulum's joint state\n", " state = self.s.get_joint_state(urdf_obj = self.pendulum,\n", " joint_name = 'chassis_to_arm')\n", "\n", " # Get the angle and angular velocity of the pendulum\n", " angle = 180. * state['position'] / 3.142\n", " angle_vel = 180. * state['velocity'] / 3.142\n", " \n", " # Apply a proportional torque\n", " torque = -angle - 0.01*angle_vel\n", " self.s.set_joint_torque(urdf_obj = self.pendulum,\n", " joint_name = 'chassis_to_arm',\n", " torque = torque,\n", " show_arrow = True,\n", " arrow_scale = 0.02,\n", " arrow_offset = 0.05)\n", " \n", " '''\n", " ##############################################################\n", " This is how we modify a plot in real time. Essentially, we \n", " identify which artist of which plot we would like to draw \n", " a data point, and then we specify that data point.\n", " Because our plot only has a single artist, there is no need \n", " to reference the artist_id\n", " ##############################################################\n", " '''\n", " # Add (angle, angle_vel) point to plot self.p\n", " self.s.add_line_datum(plot_id = self.p,\n", " x = angle,\n", " y = angle_vel)\n", "\n", " '''\n", " ##############################################################\n", " As usual, at the bottom of the run function we step the \n", " simulation.\n", " ##############################################################\n", " '''\n", " ret_code = self.s.step(max_time=max_time)\n", "\n", " '''\n", " ##############################################################\n", " Finally, when the simulation loop is over and we are done with\n", " the animator, we terminate it.\n", " ##############################################################\n", " '''\n", " self.s.terminate_animator()\n", " " ] }, { "cell_type": "markdown", "id": "420433c6-c1d2-4702-aba4-52295bf753e2", "metadata": {}, "source": [ "## Running the Project Class" ] }, { "cell_type": "markdown", "id": "da509ec5-56c1-4bc4-a9d0-0db238bd069a", "metadata": {}, "source": [ "Now that we have made the `Project` class, we can test it by initializing it and then calling the `run` function. Note: `%%capture` is required to supress the animator drawing the first frame to this notebook. Though it is not required, it is reccomended. It must be the first line in the cell in which it's called." ] }, { "cell_type": "code", "execution_count": 3, "id": "ffed964d-6581-4f93-8d9e-d55f353d342c", "metadata": {}, "outputs": [], "source": [ "%%capture \n", "# Create an instance of the Project class. \n", "proj = Project()\n", "\n", "# Run the simulation\n", "proj.run(max_time = 5.0)" ] }, { "cell_type": "markdown", "id": "36d1e2de-e3f4-4dfb-a530-766a4420f876", "metadata": {}, "source": [ "## Challenge" ] }, { "cell_type": "markdown", "id": "5ff6e017-b96d-4668-879d-5ebaf083ae88", "metadata": {}, "source": [ "This tutorial is now complete. For an additional challenge, think of how you might add another plot to the animator that draws both the angle and angular velocity as a function of time." ] } ], "metadata": { "kernelspec": { "display_name": "Python 3 (ipykernel)", "language": "python", "name": "python3" }, "language_info": { "codemirror_mode": { "name": "ipython", "version": 3 }, "file_extension": ".py", "mimetype": "text/x-python", "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", "version": "3.13.7" } }, "nbformat": 4, "nbformat_minor": 5 }