{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Four-bar Linkage\n",
"\n",
"Welcome to this tutorial on using SymPy to derive the equations of motion (EoMs) of a four-bar linkage!\n",
"\n",
"This notebook provides a hands-on introduction to using SymPy in a workflow, which is similar to the internals of SymBRiM. which also follows [the four-bar linkage example in SymPy](https://docs.sympy.org/dev/modules/physics/mechanics/examples/four_bar_linkage_example.html). In doing so it aims to satisfy the following learning objectives:\n",
"\n",
"- Learn some of the basic syntax of SymPy for creating symbols and expressions.\n",
"- Learn how to create bodies and joints in `sympy.physics.mechanics`.\n",
"- Learn how to define a system in a systematic approach such that the EoMs can automatically be formed using Kane's method.\n",
"- Learn about the underlying workflow utilized in SymBRiM.\n",
"- Learn to perform a simple simulation using `solve_ivp` from SciPy and visualize the results using SymMePlot.\n",
"\n",
"[Learn Multibody Dynamics by Moore](https://moorepants.github.io/learn-multibody-dynamics/index.html) is a great resource for learning about multibody dynamics while using the more lower-level API of the mechanics module. It should be noted that this book is part of a university course, so it does not use a lot of the high-level API of `sympy.physics.mechanics`. The following chapters are recommendable for learning more about the basics:\n",
"\n",
"- [SymPy](https://moorepants.github.io/learn-multibody-dynamics/sympy.html) gives a brief introduction to the basics of SymPy.\n",
"- [Orientation of Reference Frames](https://moorepants.github.io/learn-multibody-dynamics/orientation.html) gives a brief introduction to the basics of reference frames.\n",
"- [Vectors](https://moorepants.github.io/learn-multibody-dynamics/vectors.html) gives a brief introduction to the basics of vectors.\n",
"- [Angular Kinematics](https://moorepants.github.io/learn-multibody-dynamics/angular.html) is a more in-depth introduction to angular kinematics.\n",
"- [Translational Kinematics](https://moorepants.github.io/learn-multibody-dynamics/translational.html) introduces the usage of points and discusses various theorems for computing velocities and accelerations.\n",
"- [Holonomic Constraints](https://moorepants.github.io/learn-multibody-dynamics/configuration.html) and [Nonholonomic Constraints](https://moorepants.github.io/learn-multibody-dynamics/motion.html) introduce the usage of constraints and how to formulate them.\n",
"- [Mass Distribution](https://moorepants.github.io/learn-multibody-dynamics/mass.html) gives an in-depth introduction to defining mass and inertia.\n",
"- [Force, Moment and Torque](https://moorepants.github.io/learn-multibody-dynamics/loads.html) gives an in-depth introduction to defining forces, moments, and torques without utilizing the high-level API of `sympy.physics.mechanics`.\n",
"- Other chapters cover things like how vector differentiation is defined and how to derive the EoMs using your own manually written Kane's method and Lagrange's method.\n",
"\n",
"## Overview of the Workflow\n",
"When forming the EoMs of a system in SymPy, one typically first describes the model after which an algorithmic implementation of Kane's method or Lagrange's method is used to derive the EoMs. In this tutorial, we will use Kane's method. SymBrim breaks the model description into four parts:\n",
"\n",
"1. **Define objects**: Create the objects, such as symbols reference frames, without defining any relationships between them.\n",
"2. **Define kinematics**: Establish relationships between the objects' orientations/positions, velocities, and accelerations.\n",
"3. **Define loads**: Specifies the forces and torques acting upon the system.\n",
"4. **Define constraints**: Computes the holonomic and nonholonomic constraints to which the system is subject.\n",
"\n",
"The image below shows a schematic visualization of these steps for a rolling disc.\n",
"\n",
"
\n",
"\n",
"After describing the model and forming the EoMs, this tutorial will show how to do a simple simulation using `solve_ivp` from SciPy and visualize the results using SymMePlot.\n",
"\n",
"\n",
"## Description Four-bar Linkage\n",
"A four-bar linkage is a frequently modeled system. It consists of four rigid bodies modeled as thin rods with uniform density, where the first body is fixed to the ground. Each of these bodies is attached to the previous via a pin joint. The last pin joint is modeled as holonomic constraints when using generalized coordinates, also called a minimal coordinate representation. In the image below the four-bar linkage is shown. Additionally, we will also model the gravity force acting on the system, introduce a spring-damper element between the first and second body, and have a time-dependent force acting horizontally at the third pin joint.\n",
"\n",
"
\n",
"\n",
"## Define Objects\n",
"The first set of objects to define are the symbols, which represent various quantities in the system. In SymPy mechanics we distinguish two kinds of symbols: [symbols](https://docs.sympy.org/dev/modules/core.html#sympy.core.symbol.Symbol) and [dynamic symbols](https://docs.sympy.org/dev/modules/physics/vector/api/functions.html#sympy.physics.vector.dynamicsymbols). Symbols denote constants, like mass, dynamic symbols denote time-varying functions, like generalized coordinates. The code snippet below defines the symbols and dynamic symbols for the four-bar linkage.\n",
"\n",
"**Exercise**: Add the missing symbols and dynamic symbols for the four-bar linkage: `l1, l2, l3, l4, k, c` and `q3, u1, u2, u3`."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:27.943017Z",
"iopub.status.busy": "2024-10-12T22:55:27.942852Z",
"iopub.status.idle": "2024-10-12T22:55:28.262775Z",
"shell.execute_reply": "2024-10-12T22:55:28.262209Z"
}
},
"outputs": [],
"source": [
"import warnings\n",
"\n",
"# Import sympy under the alias sm\n",
"# Import sympy.physics.mechanics under the alias me\n",
"import sympy as sm\n",
"import sympy.physics.mechanics as me\n",
"\n",
"me.init_vprinting() # Some pretty printing setting"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:28.264886Z",
"iopub.status.busy": "2024-10-12T22:55:28.264670Z",
"iopub.status.idle": "2024-10-12T22:55:28.699701Z",
"shell.execute_reply": "2024-10-12T22:55:28.699161Z"
}
},
"outputs": [
{
"data": {
"image/png": "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",
"text/latex": [
"$\\displaystyle \\left( l_{1}, \\ l_{2}, \\ l_{3}, \\ l_{4}, \\ g, \\ k, \\ c, \\ q_{1}, \\ q_{2}, \\ q_{3}, \\ u_{1}, \\ u_{2}, \\ u_{3}\\right)$"
],
"text/plain": [
"(l₁, l₂, l₃, l₄, g, k, c, q₁, q₂, q₃, u₁, u₂, u₃)"
]
},
"execution_count": 2,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"# Define the symbols\n",
"rho, g = sm.symbols(\"rho g\")\n",
"q1, q2 = me.dynamicsymbols(\"q1:3\") # You can also use ranges.\n",
"F = sm.Function(\"F\")(sm.Symbol(\"t\")) # Is exactly the same as a dynamicsymbol.\n",
"### BEGIN SOLUTION\n",
"l1, l2, l3, l4, k, c = sm.symbols(\"l1:5 k c\")\n",
"q3, u1, u2, u3 = me.dynamicsymbols(\"q3 u1:4\")\n",
"### END SOLUTION\n",
"l1, l2, l3, l4, g, k, c, q1, q2, q3, u1, u2, u3"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Other objects we need to create are [reference frames](https://docs.sympy.org/dev/modules/physics/vector/api/classes.html#sympy.physics.vector.frame.ReferenceFrame), [points](https://docs.sympy.org/dev/modules/physics/vector/api/kinematics.html#sympy.physics.vector.point.Point), [rigid bodies](https://docs.sympy.org/dev/modules/physics/mechanics/api/body.html#sympy.physics.mechanics.rigidbody.RigidBody), and [particles](https://docs.sympy.org/dev/modules/physics/mechanics/api/particle.html#sympy.physics.mechanics.particle.Particle). In general one can state that [reference frames](https://docs.sympy.org/dev/modules/physics/vector/api/classes.html#sympy.physics.vector.frame.ReferenceFrame) and [points](https://docs.sympy.org/dev/modules/physics/vector/api/kinematics.html#sympy.physics.vector.point.Point) store the kinematic relationships, where reference frames store the rotational relations and points the translational. [Rigid bodies](https://docs.sympy.org/dev/modules/physics/mechanics/api/body.html#sympy.physics.mechanics.rigidbody.RigidBody) and [particles](https://docs.sympy.org/dev/modules/physics/mechanics/api/particle.html#sympy.physics.mechanics.particle.Particle) can mostly be seen as dataclasses, which basically store the inertial properties and the related point and frame if applicable. For the inertia it should be noted that we use [dyadics](https://docs.sympy.org/dev/modules/physics/vector/api/classes.html#sympy.physics.vector.dyadic.Dyadic), but as the code shows these can also be initialized in a tensor kind of format.\n",
"\n",
"The code snippet below defines the first link by fully specifying all initialize properties (all arguments are made keyword arguments to show what each is). We also create a point `P41` to represent the location of the closing joint w.r.t `link1`."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:28.726339Z",
"iopub.status.busy": "2024-10-12T22:55:28.725835Z",
"iopub.status.idle": "2024-10-12T22:55:28.733563Z",
"shell.execute_reply": "2024-10-12T22:55:28.733155Z"
}
},
"outputs": [
{
"data": {
"text/plain": [
"RigidBody('link1', masscenter=mc1, frame=N, mass=l1*rho, inertia=Inertia(dyadic=l1**3*rho/12*(N.z|N.z), point=mc1))"
]
},
"execution_count": 3,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"N = me.ReferenceFrame(\"N\")\n",
"mc1 = me.Point(\"mc1\")\n",
"I1 = me.Inertia.from_inertia_scalars(point=mc1, frame=N, ixx=0, iyy=0, izz=rho * l1 ** 3 / 12)\n",
"link1 = me.RigidBody(\n",
" name=\"link1\",\n",
" masscenter=mc1,\n",
" frame=N,\n",
" mass=rho * l1,\n",
" inertia=I1,\n",
")\n",
"P41 = me.Point(\"P41\")\n",
"link1"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"To define `link2` we can also make use of SymPy automatic creation of properties, where we can also overwrite them afterward."
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:28.735421Z",
"iopub.status.busy": "2024-10-12T22:55:28.734944Z",
"iopub.status.idle": "2024-10-12T22:55:28.832515Z",
"shell.execute_reply": "2024-10-12T22:55:28.832109Z"
}
},
"outputs": [
{
"data": {
"text/latex": [
"$\\displaystyle \\left[\\begin{matrix}link_{2 ixx} & link_{2 ixy} & link_{2 izx}\\\\link_{2 ixy} & link_{2 iyy} & link_{2 iyz}\\\\link_{2 izx} & link_{2 iyz} & link_{2 izz}\\end{matrix}\\right]$"
],
"text/plain": [
"⎡link₂ ᵢₓₓ link_2_ixy link_2_izx⎤\n",
"⎢ ⎥\n",
"⎢link_2_ixy link_2_iyy link_2_iyz⎥\n",
"⎢ ⎥\n",
"⎣link_2_izx link_2_iyz link_2_izz⎦"
]
},
"metadata": {},
"output_type": "display_data"
},
{
"data": {
"image/png": "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",
"text/latex": [
"$\\displaystyle \\frac{l_{2}^{3} \\rho}{12}\\mathbf{\\hat{link2_frame}_z}\\otimes \\mathbf{\\hat{link2_frame}_z}$"
],
"text/plain": [
" 3 \n",
"l₂ ⋅ρ\n",
"─────\n",
" 12 link2_frame_z⊗link2_frame_z"
]
},
"execution_count": 4,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"link2 = me.RigidBody(\"link2\", mass=rho * l2)\n",
"display(link2.central_inertia.to_matrix(link2.frame)) # Display central inertia as matrix.\n",
"link2.central_inertia = me.inertia(link2.frame, 0, 0, rho * l2 ** 3 / 12) # Overwrite central inertia.\n",
"link2.central_inertia"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"To keep track of all objects in the system, such that we can in the end also easily form the EoMs we can make use of `System`, which is not available yet in the public API. It is of course also possible to add things to the system later on."
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:28.834077Z",
"iopub.status.busy": "2024-10-12T22:55:28.833932Z",
"iopub.status.idle": "2024-10-12T22:55:28.836806Z",
"shell.execute_reply": "2024-10-12T22:55:28.836412Z"
}
},
"outputs": [],
"source": [
"from sympy.physics.mechanics.system import System\n",
"\n",
"system = System(N, link1.masscenter)\n",
"system.add_bodies(link1, link2)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"**Exercise**: Create the remaining links `link3` and `link4` and the point `P44` to represent the location of the closing joint w.r.t `link4`."
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:28.838311Z",
"iopub.status.busy": "2024-10-12T22:55:28.838141Z",
"iopub.status.idle": "2024-10-12T22:55:28.866545Z",
"shell.execute_reply": "2024-10-12T22:55:28.866128Z"
}
},
"outputs": [
{
"data": {
"text/plain": [
"(RigidBody('link1', masscenter=mc1, frame=N, mass=l1*rho, inertia=Inertia(dyadic=l1**3*rho/12*(N.z|N.z), point=mc1)),\n",
" RigidBody('link2', masscenter=link2_masscenter, frame=link2_frame, mass=l2*rho, inertia=Inertia(dyadic=l2**3*rho/12*(link2_frame.z|link2_frame.z), point=link2_masscenter)))"
]
},
"execution_count": 6,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"### BEGIN SOLUTION\n",
"link3 = me.RigidBody(\"link3\", mass=rho * l3)\n",
"link3.central_inertia = me.inertia(link3.frame, 0, 0, rho * l3 ** 3 / 12)\n",
"link4 = me.RigidBody(\"link4\", mass=rho * l4)\n",
"link4.central_inertia = me.inertia(link4.frame, 0, 0, rho * l4 ** 3 / 12)\n",
"P44 = me.Point(\"P44\")\n",
"### END SOLUTION\n",
"system.bodies"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Define Kinematics\n",
"The kinematic relations in SymPy mechanics are stored in [graphs](https://en.wikipedia.org/wiki/Graph_theory). Here the reference frames and points form the nodes and the edges are the kinematic relations. To orient frames w.r.t. each other it is advisable to use either:\n",
"\n",
"- [ReferenceFrame.orient_axis](https://docs.sympy.org/dev/modules/physics/vector/api/classes.html#sympy.physics.vector.frame.ReferenceFrame.orient_axis), which orients a frame w.r.t. another frame by rotating around an axis.\n",
"- [ReferenceFrame.orient_body_fixed](https://docs.sympy.org/dev/modules/physics/vector/api/classes.html#sympy.physics.vector.frame.ReferenceFrame.orient_body_fixed), which rotates a frame using three successive body fixed simple axis rotations.\n",
"- [ReferenceFrame.orient_explicit](https://docs.sympy.org/dev/modules/physics/vector/api/classes.html#sympy.physics.vector.frame.ReferenceFrame.orient_explicit), which allows you to specify the DCM explicitly.\n",
"\n",
"The location of points w.r.t. one another is set using [Point.set_pos](https://docs.sympy.org/dev/modules/physics/vector/api/kinematics.html#sympy.physics.vector.point.Point) and retrieved using [Point.pos_from](https://docs.sympy.org/dev/modules/physics/vector/api/kinematics.html#sympy.physics.vector.point.Point.pos_from). As for their velocity those are set w.r.t. to the reference frames.\n",
"\n",
"The code below specifies the kinematic relations of the first pin joint using this approach."
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:28.868219Z",
"iopub.status.busy": "2024-10-12T22:55:28.867909Z",
"iopub.status.idle": "2024-10-12T22:55:28.901912Z",
"shell.execute_reply": "2024-10-12T22:55:28.901417Z"
}
},
"outputs": [
{
"data": {
"image/png": "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",
"text/latex": [
"$\\displaystyle \\frac{l_{2} u_{1}}{2}\\mathbf{\\hat{link2_frame}_y}$"
],
"text/plain": [
"l₂⋅u₁ \n",
"───── link2_frame_y\n",
" 2 "
]
},
"execution_count": 7,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"link2.frame.orient_axis(link1.frame, link1.z, q1)\n",
"link2.frame.set_ang_vel(link1.frame, u1 * link1.z) # Specify the angular velocity using a generalized speed.\n",
"P1 = link1.masscenter.locatenew(\"P1\", l1 / 2 * link1.x) # Location of the first pin joint.\n",
"link2.masscenter.set_pos(P1, l2 / 2 * link2.x) # Set the position of the mass center of link2 w.r.t. P1.\n",
"link1.masscenter.set_vel(link1.frame, 0) # Fixate the mass center of link1 in its frame.\n",
"\n",
"# Add the generalized coordinate, speed, and kinematic differential equation to the system.\n",
"system.add_coordinates(q1)\n",
"system.add_speeds(u1)\n",
"system.add_kdes(u1 - q1.diff())\n",
"\n",
"link2.masscenter.vel(link1.frame) # Get the velocity of the mass center of link2 in the frame of link1."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The orientation between frames is stored as a Direction Cosine Matrix (DCM). A DCM between two frames can be computed using the [dcm](https://docs.sympy.org/dev/modules/physics/vector/api/classes.html#sympy.physics.vector.frame.ReferenceFrame.dcm) method. The following code computes the DCM, which maps vectors expressed in `link1.frame` to vectors expressed in `link2.frame`."
]
},
{
"cell_type": "code",
"execution_count": 8,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:28.903663Z",
"iopub.status.busy": "2024-10-12T22:55:28.903241Z",
"iopub.status.idle": "2024-10-12T22:55:28.911701Z",
"shell.execute_reply": "2024-10-12T22:55:28.911311Z"
}
},
"outputs": [
{
"data": {
"text/latex": [
"$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(q_{1} \\right)} & \\sin{\\left(q_{1} \\right)} & 0\\\\- \\sin{\\left(q_{1} \\right)} & \\cos{\\left(q_{1} \\right)} & 0\\\\0 & 0 & 1\\end{matrix}\\right]$"
],
"text/plain": [
"⎡cos(q₁) sin(q₁) 0⎤\n",
"⎢ ⎥\n",
"⎢-sin(q₁) cos(q₁) 0⎥\n",
"⎢ ⎥\n",
"⎣ 0 0 1⎦"
]
},
"execution_count": 8,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"link2.frame.dcm(link1.frame)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"While the above works fine, it is also possible to make use the [PinJoint](https://docs.sympy.org/dev/modules/physics/mechanics/api/joint.html#sympy.physics.mechanics.joint.PinJoint). For more information on the interface of the joint classes, refer to its [documenation](https://docs.sympy.org/dev/modules/physics/mechanics/joints.html#joints-in-physics-mechanics) and [API reference](https://docs.sympy.org/dev/modules/physics/mechanics/api/joint.html). The other joints in `sympy.physics.mechanics` are:\n",
"\n",
"- [PrismaticJoint](https://docs.sympy.org/dev/modules/physics/mechanics/api/joint.html#sympy.physics.mechanics.joint.PrismaticJoint)\n",
"- [CylindricalJoint](https://docs.sympy.org/dev/modules/physics/mechanics/api/joint.html#sympy.physics.mechanics.joint.CylindricalJoint)\n",
"- [PlanarJoint](https://docs.sympy.org/dev/modules/physics/mechanics/api/joint.html#sympy.physics.mechanics.joint.PlanarJoint)\n",
"- [SphericalJoint](https://docs.sympy.org/dev/modules/physics/mechanics/api/joint.html#sympy.physics.mechanics.joint.SphericalJoint)\n",
"- [WeldJoint](https://docs.sympy.org/dev/modules/physics/mechanics/api/joint.html#sympy.physics.mechanics.joint.WeldJoint)"
]
},
{
"cell_type": "code",
"execution_count": 9,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:28.913171Z",
"iopub.status.busy": "2024-10-12T22:55:28.913016Z",
"iopub.status.idle": "2024-10-12T22:55:29.026932Z",
"shell.execute_reply": "2024-10-12T22:55:29.026472Z"
}
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAbgAAAAcCAYAAAATMKr/AAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjkuMiwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy8hTgPZAAAACXBIWXMAABJ0AAASdAHeZh94AAAOMUlEQVR4nO2de7QcVZWHvyQgCe8AYhxcGEAJwaB5QdCBTDAaBBSSAApCTGTNIAwjEzUKUWHzywIUwWVUJuIIJrjUAMoEhiC+SHgoKoQhDpDwUAiDQzBAGOQdCHf+2Kdy69at6kd13+77ON9avbq7TvWp3dX77HPO3vucHtTR0UEkMtCQtAOwGtgLGASsASaa2cvtlCsSaQUDRf8Ht1uASKRN/BuwNzAXOBvYH7iknQJFIi1kQOj/oDiDiww0JJ0ALAXOMzOFY18DPg8cbWY3tFO+SKQnGUj6Hzu4SCQSifRLoosyEolEIv2S2MFFIpFIpF+yVfaApM8CXwdOMrMf11uhpB8AHwL2MrMXGxex/yFpArAK+Eczu6KHrzUHWJw6tJeZratW1hPXi/R/GrUfTZZlDlH3BzTdOjhgfHj+r3orkzQROBmYFzu3YszsbknXAedLutrMXig6V9JbgCfwrCcBM4CjgAOAPYBNwL14w1psZm/0sPg9giThujca2BXYAXgJeAz4DbDIzO5tn4SRGiltPwYikoYDRwOTgXHACGA34BXgYeBGYKGZbWybkH2YPBfleOAF4KES9V0I/A34TiNCDRC+givzmVXOOwb/nZYBxwPfAyYBfwAWAtcCY4DLgWskDapDhrvwzKnk0c5GdC7wYWAfYGdgCN7JjQFOA+6WdEzbpIvUSiP2o5X0Ft2fCiwBTsE7uLcCW+O6Px44B7hP0n5tkq9P02UGJ2k7YBRwR70zAUn7Ah8ALu9NiwVTboPDzOyW9krTiZndKekB4FOSLjKzzQWnzgCeAW7DF2QeDdyY/n0kfRG4EzgWmIl3erXIcD9wf/lv0VQ24DO1R3Bjsz0wDZgYyrfGBwXXN+NiknY0s781o67+QqNtpRH70Wp6me4DPAf8HJdpJ+DjeGdHeP4u8A/NuNBA0v2si3IsPlu4u0Rdp+AG+OpsQTDAFwAzzWxZpuztwDpgmZnNLHHdHqWHZb8KOA8fGPwi59o7Ae8Hfhw6wBV5lZjZk5IuC3JOocYOrihmkHN8H7yzOQ03YC8Cv8Rd0U/Ucq1wvZOAK/HZGcBP8FjNa2b2lpzzzwHWAvsm8tVxrex3eCfe+X8SX+D6O2CKpF2As/DR8juAXYBtcU/Eg/jM+dL0oC2n7lHAx0Ldb8VnLxea2dWStgUMN1hvBv4MXGJm6c+n5Z4EnAEcEup6I3zmOnq/q2osJe1Hq21EnfG5hvW/SPfxwdxc4Htm9lLq/IuA+4Ddw6FDJe1gZs+X+G4DVvezLsoJ4bmM//wDwGbg9zlliV8+T/GTEfo9Ja7ZCnpS9t+G5w8WlB8FvAn4jxrqei08v15Slkoswd3O7wGG4jGyE4GbJW1TSwU5DXwxcKKZvZZz7mBJuwInAHumihqJwX0fuAjYD7+nCX8HfAHX35HAjvjAbxfgvcDXgN+E2UkRPwIW4B3wUODdwFWSTgVuDvW/DdgG3zHi+5JmZyuRdC5ufGal6toWj7eeA9wjaVT9X71lNGI/erONWEID+l9J981shZl9M925AZjZU8DtqUOD6Kq39TBgdT87gysVIA43YCywtiC5ZALwtJn9T05Zu5W3Gj0p+13heXJB+Qx8tPirSpVI2gr4RHj785KyVOJQXFnvAKbjSgfeYKaTM2vPyJdt4N8C5ppZR+a8kcCjBdU8Q/V4ZSUOxWeD/4mPDIeF428AD+Au3ieBZ3EjMBo4Dm8j44HTKd7KaCJ+Dx4B/gWPn4C7lcB3jXgM+DSQGIuz8XsCgKTj8CSihN/iv/t2eKMfgXf2yyQdUMGl3U4aSTDpzTaitP7Xqvs5nxsU6k/4s5k9U0J2GMC6n9fBvYzfjHrYA/8B12cLwjR4JDkuuECivL0u66qnZTez5yS9QtdZSnLtofhyi5vM7JUqVX0VT8b4mZkVydoIy4BjzaxD0kI8XpY02IOo3MGdhCtvcv4FZvblOq+/FvhYNotS0puAb+OJOLvjo9xxZrY6p47f47GlLvfSzNYAoyXtARyIj2qH4TOJMeEBcATFjfxyM/unIBPA/FTZd83stFA2GB/RAuyXcTmdnfrMz4APJ0ZQ0mJ8M1xw4/MR3G3T2yhlP/qAjSir/43o/rnAuzLvtxB1vza2dHDBoI4GViU9pKQzgE/hygceAD3fzG7M1LNreH425xrJqG5VgQzjgQ1pX7akyXhm0wTcF3u8mf202peRtA54e0HxyvADpLnSzOZUqLKM7PNxf/co4FVcueab2X0FdWwEusWfcJ//9njjKkTSmcDn8JHYrErnNsB3EoUzs42SnqZT5uFVPnt+6vXnzazShq4b8d99K3zUlmRVjgbulHSKmS1NnX86cGp4vRaPHRQtT7kkb6AQ0rQX48k7lTJQ31ah7Eep1+syZem1YA9nyoYDz4dYxfjU8SOBN3L0NeEQGuzgmt1WCuxHrW2hZTaiJGX1vx7dB7Z0BJcAn0kfzllTGHW/BtIzuPeE92kf+F/w3vVh/AbMBq6TNMHM/jt1XhKEHJpzjcQv3823Lmlv3N+bdattB/wRv/k1JUwEFuIp5mnG4qOcK+n+A6yuUl8Z2acAi3D34yDcP/1rSfsXBEmH0Xn/0szA17hlBxNpGc4AvomPcKb2YALCusz7V1Ova90NZzPF7kcAQmbXFiMgaR5+f6fiunW5pJVm9mQ4JRlhPg28q4rb54GC41fg+lGNSrGW/0293pQpSychZOOjyb0bTmUDk+XNdZxbxEKa21by7McUamsLrbQRZViXeV+v/lfVfdjyFzZL8dg7QAfwhYKOMep+DaQ7uG4BYjPLpmR/SdLpeAAy3cFtCM+70p1x2XpTHBmeu/jWzewm4CbYMu2tCTNbmD0Wsn6OAZaUSH0uI/vhmevPwlOA/x64IVM2GDcyj2aOD8Gn4ivM7Lk8wSTNBb6BZ1pNNbMNeec1iWwiSD07dK/FR/ZDgKWSZprZ8lo+aGavS7oB7+DAg86TgOsl3UJn2vRuhFGfmRU1lm6j2zB6PDp1aCU+Kn7UzDZLugZfe1iNbokyKWpJ+nkWv6eJ7CsI+l/AGgBJS/FEnD+Y2cFJoaQ78DZ6lZmdmFdBD7SVPPtRa1tomY0oSVn9r1n3Je0JLKczvvcS8Akz69Z5R92vXffTHVzFAHEwuh/F3WZ3ZIrXA0/hrogs+wGvmdljmfq2wd2fhdfsBTRD9h3w0Ure7GoU/sOuzhyfjA8Wct2Tks7C426rgQ+a2dNVZGgnJ+BZaOPwtWw/lfQRM9uSOCPpMOAhM0uPBpMBwBGZ+hLjsgaPUeyGjxzLJCDsTGd8BGC5mf0pXHt34LASddaNmb0k6R462+AI4DLL7HAjaWt84PO7cGgRfn8nhVnRmhBPSRr8lbSOWhJMitpCX7YRlaiq+wCSDsbdbonb8y/439YU6XTU/Rp1P9vBvUpm8aOkA0KlQ4HngenZYH8Ivt4GHCvpHcmNCmwCtpa0r5k9FOrcDvh3OqfZvTWDshmyL8Q7orzlE8mPsTJzfCae4dRtUbN8bdgC3J0zrZeviwKPDRyOpzyPwt0d10s6wsxuDefMBk6WtBL/Xv+HN94j8RFwuq5bAczsn8ModDawPj2Kq4MN4Vo7h/dflm+N1oHHM3crUWdZLqIzWWF/4H5Jy/Dsth1xgzYlvN4LeNbMbpd0Lz7qPwWYh+vOINw9VDH7tsnk2o8MC8lvC33ZRlSiqu5Leh+eoZmEdzbjejBV0tRMfVeb2eNR92vX/cGwJSNnDHBvzrqkB3Hf/MHAZcAPJI2hO8lU+vDM8SQz6lZJi+SbMT+E+9XX4y6LR2q8Ea2mIdklXYzPxo4vSG2dhit0tiObju8G8ddMfbPxzm0z3mjOlHRe5jGnzu/Y44Q1PdOAx8OhYcBySe9NnTYEX49zFr5jyefo2rk9D5xQ5LItKdfr+PZyCcPxTK+z8Ibesg7CzK7BF/0nM9Q9gX/F78V83J20Y85HF4XnWfLlIseF9z9s1VKCKvYjOadSW+jLNqIiNej+vnTNXRiC6/7FOY99mijXgND9JNB3AD6F7uYGMLNNZvYnM1tlZvPxEdjcnItdC/yVzvVYCRfgaz86gDl4D70AXyg5AlhdJUDaTkrLLunr+L2YmpnRJuU74R3ZcjN7PHX8QDxrKc89mezkMQT/DSznMaeub9gizNc3TcOD4uCu7pvk/6xwBXApnoywHh/Rb8L16TY8RfqdIe7SbLkuxt1ga/F4wlN4ZtgkugbJexzzf1c+CF+Y+zCefPQyHqO9Fb8P46zrDvU/xGcKu+Mj2UPC8SUtEdoptB9QvS3Qt21EVaroftsYCLpf9z96S1oBPGFmJ+eUzcdHBeMr+I/rvV4HPZsC3HQkfQvfvuYw87Umeed8Gm/Uk83s9tTxC/FRy95mVjXzaiAjaQnupnnMzEa2V5r2IelSfIujF/HswrvM7KD2SuXU0haacI0+ZyMaJeq+U0338/4uJ/3hr+Jp6o/jAeKP477Qowo+8g18v7YFeECwrNDb43ujJYyUNBbYaPk7HfQaJC3C/zJoOrBR0ohQ9EISOJU0DO/Erk13boEZwB9j5xapg0V4I092iljSPlE6qaUtNFB3n7URkaZSUferreEYgU8DH8QDoQcCRxS5isJiwlnAKlXev6waE/GgcjILvDi8XtBAna3idHwwcDPubkse81LnjMQD6POyHzaz0WY2tseljPQbwszolvD2VXwT795ALW2hLH3ZRkSaRDXdr9tFGYlEeh+Svo3vBfgTM/tou+WJRFpFJd2v6KKMRCK9G/mu7UeGRwfFewZGIv2KWnS/1m2WIpFI7+R9+O4jG4BTzezONssTibSKqrofXZSRSCQS6Zf8P2lzDJJS9fXIAAAAAElFTkSuQmCC",
"text/latex": [
"$\\displaystyle \\frac{l_{3} \\left(u_{1} + u_{2}\\right)}{2}\\mathbf{\\hat{link3_frame}_y} + l_{2} u_{1}\\mathbf{\\hat{link2_frame}_y}$"
],
"text/plain": [
"l₃⋅(u₁ + u₂) \n",
"──────────── link3_frame_y + l₂⋅u₁ link2_frame_y\n",
" 2 "
]
},
"metadata": {},
"output_type": "display_data"
},
{
"data": {
"text/latex": [
"$\\displaystyle \\left[\\begin{matrix}u_{1} - \\dot{q}_{1}\\\\u_{2} - \\dot{q}_{2}\\end{matrix}\\right]$"
],
"text/plain": [
"⎡u₁ - q₁̇⎤\n",
"⎢ ⎥\n",
"⎣u₂ - q₂̇⎦"
]
},
"execution_count": 9,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"joint2 = me.PinJoint(\n",
" name=\"joint2\",\n",
" parent=link2,\n",
" child=link3,\n",
" coordinates=q2,\n",
" speeds=u2,\n",
" parent_point=link2.masscenter.locatenew(\"P2\", l2 / 2 * link2.x), # Position of the second pin joint w.r.t. to link2 as point.\n",
" child_point=-l3 / 2 * link3.x, # Position of the second pin joint w.r.t. to link3 as vector.\n",
" joint_axis=link2.z, # Axis of rotation.\n",
")\n",
"system.add_joints(joint2)\n",
"display(link3.masscenter.vel(N)) # Get the velocity of the mass center of link3 in the Newtonian frame.\n",
"system.kdes # The kinematic differential equation has been retrieved from the joint when adding it to the system."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"**Exercise**: Establish the kinematic relations for the third pin joint."
]
},
{
"cell_type": "code",
"execution_count": 10,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:29.028501Z",
"iopub.status.busy": "2024-10-12T22:55:29.028339Z",
"iopub.status.idle": "2024-10-12T22:55:29.122608Z",
"shell.execute_reply": "2024-10-12T22:55:29.122024Z"
}
},
"outputs": [
{
"data": {
"image/png": "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",
"text/latex": [
"$\\displaystyle \\frac{l_{4} \\left(u_{1} + u_{2} + u_{3}\\right)}{2}\\mathbf{\\hat{link4_frame}_y} + l_{3} \\left(u_{1} + u_{2}\\right)\\mathbf{\\hat{link3_frame}_y} + l_{2} u_{1}\\mathbf{\\hat{link2_frame}_y}$"
],
"text/plain": [
"l₄⋅(u₁ + u₂ + u₃) ↪\n",
"───────────────── link4_frame_y + l₃⋅(u₁ + u₂) link3_frame_y + l₂⋅u₁ link2_fra ↪\n",
" 2 ↪\n",
"\n",
"↪ \n",
"↪ me_y\n",
"↪ "
]
},
"execution_count": 10,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"### BEGIN SOLUTION\n",
"joint3 = me.PinJoint(\"joint3\", link3, link4, q3, u3, l3 / 2 * link3.x, -l4 / 2 * link4.x, joint_axis=link3.z)\n",
"system.add_joints(joint3)\n",
"### END SOLUTION\n",
"# A few sanity checks.\n",
"assert len(system.q) == 3\n",
"assert len(system.u) == 3\n",
"assert len(system.kdes) == 3\n",
"link4.masscenter.vel(N) # Get the velocity of the mass center of link4 in the Newtonian frame."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"**Exercise**: Set the position of `P41` w.r.t. `link1` and `P44` w.r.t. `link4`."
]
},
{
"cell_type": "code",
"execution_count": 11,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:29.124388Z",
"iopub.status.busy": "2024-10-12T22:55:29.124223Z",
"iopub.status.idle": "2024-10-12T22:55:29.178914Z",
"shell.execute_reply": "2024-10-12T22:55:29.178379Z"
}
},
"outputs": [
{
"data": {
"image/png": "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",
"text/latex": [
"$\\displaystyle l_{4}\\mathbf{\\hat{link4_frame}_x} + l_{3}\\mathbf{\\hat{link3_frame}_x} + l_{2}\\mathbf{\\hat{link2_frame}_x} + l_{1}\\mathbf{\\hat{n}_x}$"
],
"text/plain": [
"l₄ link4_frame_x + l₃ link3_frame_x + l₂ link2_frame_x + l₁ n_x"
]
},
"execution_count": 11,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"### BEGIN SOLUTION\n",
"P41.set_pos(link1.masscenter, -l1 / 2 * link1.x)\n",
"P44.set_pos(link4.masscenter, l4 / 2 * link4.x)\n",
"### END SOLUTION\n",
"assert P44.pos_from(P41) == l1 * link1.x + l2 * link2.x + l3 * link3.x + l4 * link4.x\n",
"P44.pos_from(P41) # Get the position of P44 w.r.t. P41."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Define Loads\n",
"With the kinematics defined, the next step is to define the loads. The term loads is used as a general term for both forces and torques. A load within SymPy is defined as a vector, which is associated with a location. For a force this location must be a point and for a torque the location must be a reference frame.\n",
"\n",
"The code below defines the gravity force acting on the system using `System.apply_uniform_gravity`."
]
},
{
"cell_type": "code",
"execution_count": 12,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:29.180741Z",
"iopub.status.busy": "2024-10-12T22:55:29.180411Z",
"iopub.status.idle": "2024-10-12T22:55:29.185437Z",
"shell.execute_reply": "2024-10-12T22:55:29.185038Z"
}
},
"outputs": [
{
"data": {
"text/plain": [
"(Force(point=mc1, force=- g*l1*rho*N.y),\n",
" Force(point=link2_masscenter, force=- g*l2*rho*N.y),\n",
" Force(point=link3_masscenter, force=- g*l3*rho*N.y),\n",
" Force(point=link4_masscenter, force=- g*l4*rho*N.y))"
]
},
"execution_count": 12,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"system.apply_uniform_gravity(-g * N.y)\n",
"system.loads # Get the loads of the system."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"**Exercise**: Apply the force acting horizontally at the third pin joint with a magnitude of `F(t)` using [me.Force](https://docs.sympy.org/dev/modules/physics/mechanics/api/part_bod.html#sympy.physics.mechanics.loads.Force)."
]
},
{
"cell_type": "code",
"execution_count": 13,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:29.187015Z",
"iopub.status.busy": "2024-10-12T22:55:29.186684Z",
"iopub.status.idle": "2024-10-12T22:55:29.190564Z",
"shell.execute_reply": "2024-10-12T22:55:29.190174Z"
}
},
"outputs": [
{
"data": {
"text/plain": [
"Force(point=joint3_link3_joint, force=F(t)*N.x)"
]
},
"execution_count": 13,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"### BEGIN SOLUTION\n",
"system.add_loads(\n",
" me.Force(joint3.parent_point, F * N.x)\n",
")\n",
"### END SOLUTION\n",
"system.loads[-1]"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"**Exercise**: Apply the spring-damper element between `link1` and `link2` using [me.TorqueActuator](https://docs.sympy.org/dev/modules/physics/mechanics/api/actuator.html#sympy.physics.mechanics.actuator.TorqueActuator) (you could use the class method `at_pin_joint` for simplicity)."
]
},
{
"cell_type": "code",
"execution_count": 14,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:29.192160Z",
"iopub.status.busy": "2024-10-12T22:55:29.191794Z",
"iopub.status.idle": "2024-10-12T22:55:29.196298Z",
"shell.execute_reply": "2024-10-12T22:55:29.195797Z"
}
},
"outputs": [
{
"data": {
"text/plain": [
"(TorqueActuator(-c*u2(t) - k*q2(t), axis=link2_frame.z, target_frame=link2_frame, reaction_frame=N),)"
]
},
"execution_count": 14,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"### BEGIN SOLUTION\n",
"system.add_actuators(\n",
" me.TorqueActuator(\n",
" torque=-k * q2 - c * u2,\n",
" axis=joint2.joint_axis,\n",
" target_frame=link2,\n",
" reaction_frame=link1\n",
" )\n",
")\n",
"### END SOLUTION\n",
"system.actuators"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Define Constraints\n",
"At this state we have defined a 3-link pendulum with a spring-damper element between the first and second link and a time-dependent force acting horizontally at the third pin joint. The next step is to define the constraints. The `System` class distinguishes between holonomic and nonholonomic constraints. Holonomic constraints are constraints that can be written as a function of the generalized coordinates and time. Nonholonomic constraints are constraints also dependent on the generalized speeds and are non-integrable.\n",
"\n",
"The code below defines the holonomic constraints in the horizontal direction for the four-bar linkage."
]
},
{
"cell_type": "code",
"execution_count": 15,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:29.198057Z",
"iopub.status.busy": "2024-10-12T22:55:29.197655Z",
"iopub.status.idle": "2024-10-12T22:55:29.215264Z",
"shell.execute_reply": "2024-10-12T22:55:29.214774Z"
}
},
"outputs": [
{
"data": {
"text/latex": [
"$\\displaystyle \\left[\\begin{matrix}l_{1} + l_{2} \\cos{\\left(q_{1} \\right)} + l_{3} \\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}\\right) + l_{4} \\left(\\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\cos{\\left(q_{3} \\right)} + \\left(- \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} - \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\sin{\\left(q_{3} \\right)}\\right)\\end{matrix}\\right]$"
],
"text/plain": [
"[l₁ + l₂⋅cos(q₁) + l₃⋅(-sin(q₁)⋅sin(q₂) + cos(q₁)⋅cos(q₂)) + l₄⋅((-sin(q₁)⋅sin ↪\n",
"\n",
"↪ (q₂) + cos(q₁)⋅cos(q₂))⋅cos(q₃) + (-sin(q₁)⋅cos(q₂) - sin(q₂)⋅cos(q₁))⋅sin(q ↪\n",
"\n",
"↪ ₃))]"
]
},
"execution_count": 15,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"system.add_holonomic_constraints(\n",
" P44.pos_from(P41).dot(N.x)\n",
")\n",
"system.holonomic_constraints"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"**Exercise**: Define the holonomic constraints in the vertical direction for the four-bar linkage."
]
},
{
"cell_type": "code",
"execution_count": 16,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:29.216784Z",
"iopub.status.busy": "2024-10-12T22:55:29.216514Z",
"iopub.status.idle": "2024-10-12T22:55:29.241348Z",
"shell.execute_reply": "2024-10-12T22:55:29.240872Z"
}
},
"outputs": [
{
"data": {
"text/latex": [
"$\\displaystyle \\left[\\begin{matrix}l_{1} + l_{2} \\cos{\\left(q_{1} \\right)} + l_{3} \\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}\\right) + l_{4} \\left(\\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\cos{\\left(q_{3} \\right)} + \\left(- \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} - \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\sin{\\left(q_{3} \\right)}\\right)\\\\l_{2} \\sin{\\left(q_{1} \\right)} + l_{3} \\left(\\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} + \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{1} \\right)}\\right) + l_{4} \\left(\\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\sin{\\left(q_{3} \\right)} + \\left(\\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} + \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\cos{\\left(q_{3} \\right)}\\right)\\end{matrix}\\right]$"
],
"text/plain": [
"⎡l₁ + l₂⋅cos(q₁) + l₃⋅(-sin(q₁)⋅sin(q₂) + cos(q₁)⋅cos(q₂)) + l₄⋅((-sin(q₁)⋅sin ↪\n",
"⎢ ↪\n",
"⎣ l₂⋅sin(q₁) + l₃⋅(sin(q₁)⋅cos(q₂) + sin(q₂)⋅cos(q₁)) + l₄⋅((-sin(q₁)⋅sin(q₂ ↪\n",
"\n",
"↪ (q₂) + cos(q₁)⋅cos(q₂))⋅cos(q₃) + (-sin(q₁)⋅cos(q₂) - sin(q₂)⋅cos(q₁))⋅sin(q ↪\n",
"↪ ↪\n",
"↪ ) + cos(q₁)⋅cos(q₂))⋅sin(q₃) + (sin(q₁)⋅cos(q₂) + sin(q₂)⋅cos(q₁))⋅cos(q₃)) ↪\n",
"\n",
"↪ ₃))⎤\n",
"↪ ⎥\n",
"↪ ⎦"
]
},
"execution_count": 16,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"### BEGIN SOLUTION\n",
"system.add_holonomic_constraints(\n",
" P44.pos_from(P41).dot(N.y)\n",
")\n",
"### END SOLUTION\n",
"assert len(system.holonomic_constraints) == 2\n",
"system.holonomic_constraints"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Form EoMs\n",
"Before forming the EoMs it is necessary to specify which generalized coordinates and generalized speeds are independent and which are dependent. A useful feature to check if you have forgotten these kinds of things is `System.validate_system`."
]
},
{
"cell_type": "code",
"execution_count": 17,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:29.242967Z",
"iopub.status.busy": "2024-10-12T22:55:29.242659Z",
"iopub.status.idle": "2024-10-12T22:55:29.288035Z",
"shell.execute_reply": "2024-10-12T22:55:29.287557Z"
}
},
"outputs": [
{
"data": {
"text/plain": [
"ValueError('\\nThe number of dependent generalized coordinates 0 should be equal to\\nthe number of holonomic constraints 2.\\n\\nThe number of dependent generalized speeds 0 should be equal to the\\nnumber of velocity constraints 2.')"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"try:\n",
" system.validate_system()\n",
"except ValueError as e:\n",
" display(e)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"To set the independent and dependent generalized coordinates and speeds we can specify `System.q_ind`, `System.q_dep`, `System.u_ind`, `System.u_dep`. The code below specifies the generalized coordinates."
]
},
{
"cell_type": "code",
"execution_count": 18,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:29.289723Z",
"iopub.status.busy": "2024-10-12T22:55:29.289352Z",
"iopub.status.idle": "2024-10-12T22:55:29.303764Z",
"shell.execute_reply": "2024-10-12T22:55:29.303336Z"
}
},
"outputs": [
{
"data": {
"text/latex": [
"$\\displaystyle \\left[\\begin{matrix}q_{1}\\end{matrix}\\right]$"
],
"text/plain": [
"[q₁]"
]
},
"metadata": {},
"output_type": "display_data"
},
{
"data": {
"text/latex": [
"$\\displaystyle \\left[\\begin{matrix}q_{2}\\\\q_{3}\\end{matrix}\\right]$"
],
"text/plain": [
"⎡q₂⎤\n",
"⎢ ⎥\n",
"⎣q₃⎦"
]
},
"metadata": {},
"output_type": "display_data"
},
{
"data": {
"text/latex": [
"$\\displaystyle \\left[\\begin{matrix}q_{1}\\\\q_{2}\\\\q_{3}\\end{matrix}\\right]$"
],
"text/plain": [
"⎡q₁⎤\n",
"⎢ ⎥\n",
"⎢q₂⎥\n",
"⎢ ⎥\n",
"⎣q₃⎦"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"system.q_ind = [q1]\n",
"system.q_dep = [q2, q3]\n",
"display(system.q_ind)\n",
"display(system.q_dep)\n",
"display(system.q)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"**Exercise**: Specify the generalized speeds."
]
},
{
"cell_type": "code",
"execution_count": 19,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:29.305345Z",
"iopub.status.busy": "2024-10-12T22:55:29.305041Z",
"iopub.status.idle": "2024-10-12T22:55:29.320540Z",
"shell.execute_reply": "2024-10-12T22:55:29.320035Z"
}
},
"outputs": [
{
"data": {
"text/latex": [
"$\\displaystyle \\left[\\begin{matrix}u_{1}\\end{matrix}\\right]$"
],
"text/plain": [
"[u₁]"
]
},
"metadata": {},
"output_type": "display_data"
},
{
"data": {
"text/latex": [
"$\\displaystyle \\left[\\begin{matrix}u_{2}\\\\u_{3}\\end{matrix}\\right]$"
],
"text/plain": [
"⎡u₂⎤\n",
"⎢ ⎥\n",
"⎣u₃⎦"
]
},
"metadata": {},
"output_type": "display_data"
},
{
"data": {
"text/latex": [
"$\\displaystyle \\left[\\begin{matrix}u_{1}\\\\u_{2}\\\\u_{3}\\end{matrix}\\right]$"
],
"text/plain": [
"⎡u₁⎤\n",
"⎢ ⎥\n",
"⎢u₂⎥\n",
"⎢ ⎥\n",
"⎣u₃⎦"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"### BEGIN SOLUTION\n",
"system.u_ind = [u1]\n",
"system.u_dep = [u2, u3]\n",
"### END SOLUTION\n",
"display(system.u_ind)\n",
"display(system.u_dep)\n",
"display(system.u)\n",
"system.validate_system()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Now that we have described the model entirely we can form the equations using `System.form_eoms`. This method will automatically make use of [KanesMethod](https://docs.sympy.org/dev/modules/physics/mechanics/api/kane_lagrange.html#sympy.physics.mechanics.kane.KanesMethod) to form the EoMs. The code below forms the EoMs."
]
},
{
"cell_type": "code",
"execution_count": 20,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:29.321948Z",
"iopub.status.busy": "2024-10-12T22:55:29.321805Z",
"iopub.status.idle": "2024-10-12T22:55:29.578818Z",
"shell.execute_reply": "2024-10-12T22:55:29.578290Z"
}
},
"outputs": [],
"source": [
"with warnings.catch_warnings():\n",
" warnings.simplefilter(\"ignore\")\n",
" eoms = system.form_eoms(constraint_solver=\"CRAMER\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Solving Initial Conditions\n",
"Now that we have the equations describing the system's kinematics and dynamics, we need to convert them to fast numeric code. This step is called code generation. SymPy has an entirely [separate module](https://docs.sympy.org/dev/modules/codegen.html) that allows you to generate code in a variety of languages. In this tutorial, we will only use the [lambdify](https://docs.sympy.org/dev/modules/utilities/lambdify.html#sympy.utilities.lambdify.lambdify) function, which generates a Python function that uses NumPy by default.\n",
"\n",
"Before we can simulate the system we need to determine the initial conditions. These initial conditions should satisfy the holonomic and nonholonomic constraints. To compute the values the general workflow is to first create functions to quickly evaluate the constraints. After which we can use SciPy's [fsolve](https://docs.scipy.org/doc/scipy/reference/generated/scipy.optimize.fsolve.html) to find the values for the dependent generalized coordinates and speeds.\n",
"\n",
"The cell below specifies the constants and the initial values of the independent generalized coordinates and speeds. Next, it creates a function to quickly evaluate the holonomic constraints. Lastly, it solves for the dependent generalized coordinates."
]
},
{
"cell_type": "code",
"execution_count": 21,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:29.580499Z",
"iopub.status.busy": "2024-10-12T22:55:29.580348Z",
"iopub.status.idle": "2024-10-12T22:55:29.748367Z",
"shell.execute_reply": "2024-10-12T22:55:29.747758Z"
}
},
"outputs": [
{
"data": {
"image/png": "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",
"text/latex": [
"$\\displaystyle \\left[ -0.5, \\ -2.24641553739307, \\ -1.49782726919522\\right]$"
],
"text/plain": [
"[-0.5, -2.2464155373930677, -1.4978272691952248]"
]
},
"execution_count": 21,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"import numpy as np\n",
"from scipy.integrate import solve_ivp\n",
"from scipy.optimize import fsolve\n",
"\n",
"q_ind_vals = [-0.5]\n",
"u_ind_vals = [-0.1]\n",
"constants = {\n",
" rho: 100.0, l1: 0.8, l2: 0.5, l3: 1.0, l4: 0.7, k: 100, c: 10, g: 9.81\n",
"}\n",
"p, p_vals = zip(*constants.items()) # Get a tuple of symbols and values.\n",
"\n",
"# Always enable Common Subexpression Elimination (CSE) because it is cheap and faster.\n",
"# [:] is a quick method to convert a SymPy matrix to a list.\n",
"eval_hc = sm.lambdify((system.q_dep, system.q_ind, p), system.holonomic_constraints[:], cse=True)\n",
"\n",
"q_dep_vals = fsolve(eval_hc, [-1.5, -1.5], args=(q_ind_vals, p_vals))\n",
"q0 = [*q_ind_vals, *q_dep_vals]\n",
"q0"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We have now computed the initial values for the dependent generalized coordinates. For the dependent generalized speeds we have to do the same. These dependent generalized speeds should satisfy the constraints in the velocity space, i.e. velocity constraints. The velocity constraints are simply the combination of the time-derivatives of the holonomic constraints and the nonholonomic constraints.\n",
"\n",
"The cell below computes the velocity constraints, where it replaces the time-derivatives of the generalized coordinates with the correct generalized speeds."
]
},
{
"cell_type": "code",
"execution_count": 22,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:29.750378Z",
"iopub.status.busy": "2024-10-12T22:55:29.749911Z",
"iopub.status.idle": "2024-10-12T22:55:29.782312Z",
"shell.execute_reply": "2024-10-12T22:55:29.781748Z"
}
},
"outputs": [
{
"data": {
"image/png": "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",
"text/latex": [
"$\\displaystyle \\left\\{q_{1}, q_{2}, q_{3}, u_{1}, u_{2}, u_{3}\\right\\}$"
],
"text/plain": [
"{q₁, q₂, q₃, u₁, u₂, u₃}"
]
},
"execution_count": 22,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"assert me.dynamicsymbols._t == sm.Symbol(\"t\") # This is the same.\n",
"vel_constrs = system.holonomic_constraints.diff(me.dynamicsymbols._t).col_join(system.nonholonomic_constraints)\n",
"vel_constrs = vel_constrs.xreplace(system.eom_method.kindiffdict())\n",
"set().union(*(me.find_dynamicsymbols(vc) for vc in vel_constrs))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"**Exercise**: Code generate a function to solve the velocity constraints, compute the dependent generalized speeds, and create a list `u0` with the initial values of all generalized speeds."
]
},
{
"cell_type": "code",
"execution_count": 23,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:29.784103Z",
"iopub.status.busy": "2024-10-12T22:55:29.783784Z",
"iopub.status.idle": "2024-10-12T22:55:29.836512Z",
"shell.execute_reply": "2024-10-12T22:55:29.836079Z"
}
},
"outputs": [
{
"data": {
"image/png": "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",
"text/latex": [
"$\\displaystyle \\left[ -0.1, \\ 0.0715829929380234, \\ -0.0274688413154955\\right]$"
],
"text/plain": [
"[-0.1, 0.07158299293802342, -0.02746884131549546]"
]
},
"execution_count": 23,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"### BEGIN SOLUTION\n",
"eval_vc = sm.lambdify((system.u_dep, system.u_ind, system.q, p), vel_constrs[:], cse=True)\n",
"u_dep_vals = fsolve(eval_vc, [0.5, -0.5], args=(u_ind_vals, q0, p_vals))\n",
"u0 = [*u_ind_vals, *u_dep_vals]\n",
"### END SOLUTION\n",
"assert np.allclose(u0, [-0.1, 0.07, -0.03], atol=0.01)\n",
"u0"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"For our simulation, we of course also need to define our inputs, which is in this case just the time-dependent force acting horizontally at the third pin joint. The code below sets this force to be equal to $\\sin(t)$, but feel free to modify it."
]
},
{
"cell_type": "code",
"execution_count": 24,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:29.838267Z",
"iopub.status.busy": "2024-10-12T22:55:29.837945Z",
"iopub.status.idle": "2024-10-12T22:55:29.840744Z",
"shell.execute_reply": "2024-10-12T22:55:29.840371Z"
}
},
"outputs": [],
"source": [
"r = [F] # Inputs to the system\n",
"\n",
"def r_eval(t: float) -> list[float]:\n",
" \"\"\"Evaluate the inputs to the system.\"\"\"\n",
" return [100 * np.sin(np.pi * t)]"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Simulation\n",
"An important aspect of integrators is their suitability for solving either Ordinary Differential Equations (ODEs) or Differential Algebraic Equations (DAEs). ODEs are expressed as $\\dot{x}(t) = F(x(t), t)$, while DAEs take the form of $F(\\dot{x}(t), x(t), t) = 0$.\n",
"\n",
"In this tutorial, we'll focus on ODEs as they are easier to implement. However, it's worth noting that ODEs may eventually lead to the violation of holonomic and nonholonomic constraints. DAE integrators can maintain these constraints at zero, but they involve a more complex setup. You can access a variety of ODE integrators using the Python function [solve_ivp](https://docs.scipy.org/doc/scipy/reference/generated/scipy.integrate.solve_ivp.html) from SciPy, and for DAE integrators, consider using [dae](https://bmcage.github.io/odes/dev/dae.html) from ODES SciKits.\n",
"\n",
"To use `solve_ivp` we need to create a function which evaluates $F(x(t), t)$, where $x(t)=\\begin{bmatrix} q(t) \\\\ u(t)\\end{bmatrix}$. In the code below we create this function by first lambdifying the full mass matrix and full forcing vector, which are then evaluated and solved for $\\dot{x}(t)$ in `eval_rhs`."
]
},
{
"cell_type": "code",
"execution_count": 25,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:29.842343Z",
"iopub.status.busy": "2024-10-12T22:55:29.842038Z",
"iopub.status.idle": "2024-10-12T22:55:29.913462Z",
"shell.execute_reply": "2024-10-12T22:55:29.913023Z"
}
},
"outputs": [
{
"data": {
"text/plain": [
"array([-0.1 , 0.07158299, -0.02746884, -9.26918156, 6.63703663,\n",
" -2.55110976])"
]
},
"execution_count": 25,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"x = system.q.col_join(system.u)\n",
"x0 = np.concatenate((q0, u0))\n",
"eval_sys = sm.lambdify((x, p, r, me.dynamicsymbols._t), (system.mass_matrix_full, system.forcing_full), cse=True)\n",
"\n",
"def eval_rhs(t: float, x: np.ndarray[np.float64]) -> np.ndarray[np.float64]:\n",
" \"\"\"Evaluate the right-hand side of the system of equations.\"\"\"\n",
" mass_matrix, forcing_vector = eval_sys(x, p_vals, r_eval(t), t)\n",
" return np.linalg.solve(mass_matrix, forcing_vector).ravel()\n",
"\n",
"eval_rhs(0.0, x0)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"With everything set and ready we can solve the system for the initial conditions."
]
},
{
"cell_type": "code",
"execution_count": 26,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:29.915249Z",
"iopub.status.busy": "2024-10-12T22:55:29.914913Z",
"iopub.status.idle": "2024-10-12T22:55:29.923753Z",
"shell.execute_reply": "2024-10-12T22:55:29.923249Z"
}
},
"outputs": [],
"source": [
"sol = solve_ivp(eval_rhs, (0, 4), x0)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Visualization\n",
"To quickly visualize a system in matplotlib we can use [SymMePlot](https://github.com/TJStienstra/symmeplot), a tool to easily make 3D visualizations of mechanical systems from `sympy.physics.mechanics` in `matplotlib`."
]
},
{
"cell_type": "code",
"execution_count": 27,
"metadata": {
"execution": {
"iopub.execute_input": "2024-10-12T22:55:29.925617Z",
"iopub.status.busy": "2024-10-12T22:55:29.925310Z",
"iopub.status.idle": "2024-10-12T22:55:36.164039Z",
"shell.execute_reply": "2024-10-12T22:55:36.163467Z"
}
},
"outputs": [
{
"data": {
"text/html": [
"\n",
"\n",
"\n",
"\n",
"\n",
"\n",
"
\n",
"\n",
"\n",
"\n"
],
"text/plain": [
""
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"import matplotlib.pyplot as plt\n",
"from IPython.display import HTML\n",
"from matplotlib.animation import FuncAnimation\n",
"from scipy.interpolate import CubicSpline\n",
"from symmeplot.matplotlib import Scene3D\n",
"\n",
"# Create an interpolation function for animation purposes.\n",
"x_eval = CubicSpline(sol.t, sol.y.T)\n",
"\n",
"# Get the location of the third pin joint.\n",
"joint3_point = joint3.parent_point\n",
"\n",
"# Create a figure and scene.\n",
"fig, ax = plt.subplots(subplot_kw={\"projection\": \"3d\", \"proj_type\": \"ortho\"}, figsize=(10, 10))\n",
"scene = Scene3D(system.frame, system.fixed_point, ax=ax)\n",
"# Add the bodies to the scene.\n",
"for body in system.bodies:\n",
" scene.add_body(body)\n",
"# Add the four-bar linkage as a line.\n",
"scene.add_line([P41, P1, joint2.parent_point, joint3_point, P44], name=\"four-bar linkage\", color=\"k\")\n",
"# Add the force vector.\n",
"F_max = max(r_eval(ti)[0] for ti in sol.t)\n",
"scene.add_vector(F * N.x / F_max, joint3_point, color=\"r\")\n",
"# Setup the plotter for evaluation and plot the first frame.\n",
"scene.lambdify_system((x, r, p))\n",
"scene.evaluate_system(x_eval(0.0), r_eval(0.0), p_vals)\n",
"scene.plot()\n",
"# Some extra configurations.\n",
"ax.axis(\"off\")\n",
"ax.view_init(90, -90, 0)\n",
"\n",
"# Create the animation.\n",
"fps = 30\n",
"ani = scene.animate(\n",
" lambda ti: (x_eval(ti), r_eval(ti), p_vals),\n",
" frames=np.arange(0, sol.t[-1], 1 / fps),\n",
" blit=False)\n",
"display(HTML(ani.to_jshtml(fps=fps)))\n",
"plt.close()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"You have come to the end of this tutorial. Congratulations!\n",
"As you may have noticed the animation above clearly shows that the holonomic constraints are violated during the simulation. In future tutorials we will be using a utility, which allows the easy usage of a DAE solver to prevent this problem."
]
}
],
"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.11.10"
}
},
"nbformat": 4,
"nbformat_minor": 4
}
![\"Modeling](\"../_static/definition_steps.svg\")
![\"Four-bar](\"../_static/four_bar_linkage.svg\")