Skip to content

Instantly share code, notes, and snippets.

@mjfwest

mjfwest/ConstraintTest.py

Created February 1, 2018 13:44
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save mjfwest/3e8f5fcb0f04b77ac5edbd41f1aede33 to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save mjfwest/3e8f5fcb0f04b77ac5edbd41f1aede33 to your computer and use it in GitHub Desktop.
Download ZIP
A non functional test of configuration constraints with pydy
Raw
ConstraintTest.py
# coding: utf-8
# In[1]:
from __future__ import print_function, division
from sympy import symbols, simplify
import sympy.physics.mechanics as mech
# import dynamicsymbols, ReferenceFrame, Point, Particle
# from sympy import symbols, atan, Matrix, solve
import sympy as sym
# In[2]:
from sympy.physics.vector import init_vprinting, vlatex
init_vprinting(pretty_print=True)
# First we need to create the dynamicsymbols needed to describe the system as shown in the above diagram. In this case, the generalized coordinates $q_1$ and $q_2$ represent the mass $x$ and $y$ coordinates in the inertial $\bf{N}$ frame. Likewise, the generalized speeds $u_1$ and $u_2$ represent the velocities in these directions. We also create some symbols to represent the length and mass of the pendulum, as well as gravity and time.
# In[3]:
# Create generalized coordinates and speeds for this non-minimal realization
# qx, qy = N.x and N.y coordinates of pendulum
# u1, u2 = N.x and N.y velocities of pendulum
qx, qy = mech.dynamicsymbols('q_x, q_y')
qxd, qyd = mech.dynamicsymbols('q_x, q_y', level=1)
ux, uy = mech.dynamicsymbols('u_x, u_y')
uxd, uyd = mech.dynamicsymbols('u_x, u_y', level=1)
L, m, g, t = sym.symbols('L, m, g, t')
# Next, we create a world coordinate frame $\bf{N}$, and its origin point $N^*$. The velocity of the origin is set to 0. A second coordinate frame $\bf{A}$ is oriented such that its x-axis is along the pendulum (as shown in the diagram above).
# In[4]:
# Compose world frame
N = mech.ReferenceFrame('N')
pN = mech.Point('N*')
pN.set_vel(N, 0)
# A.x is along the pendulum
theta1 = sym.atan(qy/qx)
A = N.orientnew('A', 'axis', [theta1, N.z])
# Locating the pendulum mass is then as easy as specifying its location with in terms of its x and y coordinates in the world frame. A Particle object is then created to represent the mass at this location.
# In[5]:
# Locate the pendulum mass
P = pN.locatenew('P1', qx*N.x + qy*N.y)
pP = mech.Particle('pP', P, m)
# The kinematic differential equations (KDEs) relate the derivatives of the generalized coordinates to the generalized speeds. In this case the speeds are the derivatives, so these are simple. A dictionary is also created to map $\dot{q}$ to $u$:
# In[6]:
# Calculate the kinematic differential equations
kde = sym.Matrix([qxd - ux,
qyd - uy])
dq_dict = sym.solve(kde, [qxd, qyd])
# The velocity of the mass is then the time derivative of the position from the origin $N^{∗}$:
# In[7]:
# Set velocity of point P
P.set_vel(N, P.pos_from(pN).dt(N).subs(dq_dict))
# As this system has more coordinates than degrees of freedom, constraints are needed. The configuration constraints relate the coordinates to each other. In this case the constraint is that the distance from the origin to the mass is always the length $L$ (the pendulum doesn’t get longer). Likewise, the velocity constraint is that the mass velocity in the $A.x$ direction is always 0 (no radial velocity).
# In[8]:
f_c = sym.Matrix([P.pos_from(pN).magnitude() - L])
f_v = sym.Matrix([P.vel(N).express(A).dot(A.x)])
f_v.simplify()
# In[9]:
print(f_v)
# The force on the system is just gravity, at point P.
# In[10]:
# Input the force resultant at P
point_gravity_force = (P, m*g*N.x)
point_gravity_force
# In[11]:
thrust = mech.dynamicsymbols('T')
point_thrust_force = (P, thrust*A.y)
point_thrust_force
# With the problem setup, the equations of motion can be generated using the KanesMethod class.
# At the bare minimum for unconstrained systems, the KanesMethod class needs to know the generalized coordinates, the generalized speeds, the kinematical differential equations, the loads, the bodies, and a Newtonian reference frame. First, make a list of the generalized coordinates, i.e. the three joint angles
#
# As there are constraints, dependent and independent coordinates need to be provided to the class. In this case we’ll use $q_2$ and $u_2$ as the independent coordinates and speeds:
# In[12]:
coordinates = [qy, qx]
speeds = [ux, uy]
# In[13]:
# Derive the equations of motion using the KanesMethod class.
kane = mech.KanesMethod(N,
q_ind=[qy],
u_ind=[uy],
q_dependent=[qx],
u_dependent=[ux],
configuration_constraints=f_c,
velocity_constraints=f_v,
kd_eqs=kde)
# The equations of motion can now be computed using the `kanes_equations` method, which takes the list of loads and bodies. It returns the equations of motion (i.e. first order ordinary differential equations) in Kane's form:
#
# $$ F_r + F_r^* = 0$$
#
# which is essentially equivalent to the classic Newton_Euler form:
#
# $$ \mathbf{F} = \mathbf{m}\mathbf{a} $$
#
# $$ \mathbf{T} = \mathbf{I} \mathbf{\alpha} $$
# In[14]:
loads = [point_gravity_force,
point_thrust_force] # gravitational force R at point P
bodies = [pP] # Particle P
loads
# In[15]:
(fr, frstar) = kane.kanes_equations(loads, bodies)
sym.trigsimp(fr + frstar)
# Keep in mind that out utlimate goal is to have the equations of motion in first order form:
#
# $$ \dot{\mathbf{x}} = \mathbf{g}(\mathbf{x}, t) $$
#
# The equations of motion are linear in terms of the derivatives of the generalized speeds and the `KanesMethod` class automatically puts the equations in a more useful form for the next step of numerical simulation:
#
# $$ \mathbf{M}(\mathbf{x}, t)\dot{\mathbf{x}} = \mathbf{f}(\mathbf{x}, t) $$
#
# Note that
#
# $$ \mathbf{g} = \mathbf{M}^{-1}(\mathbf{x}, t) \mathbf{f}(\mathbf{x}, t) $$
#
# and that $\mathbf{g}$ can be computed analytically but for non-toy problems, it is best to do this numerically. So we will simply generate the $\mathbf{M}$ and $\mathbf{f}$ matrices for later use.
#
# The mass matrix, $\mathbf{M}$, can be accessed with the `mass_matrix` method (use `mass_matrix_full` to include the kinematical differential equations too. We can use `trigsimp` again to make this relatively compact:
# In[16]:
mass_matrix = sym.trigsimp(kane.mass_matrix_full)
print(mass_matrix)
# The right hand side, $\mathbf{f}$, is a vector function of all the non-inertial forces (gyroscopic, external, coriolis, etc):
# In[17]:
forcing_vector = sym.trigsimp(kane.forcing_full)
print(forcing_vector)
# ## Simulation
# Now that we have the symbolic equations of motion we need to transform them into Python functions that can be evaluated for use in numerical integration. [Numerical integration](http://en.wikipedia.org/wiki/Numerical_methods_for_ordinary_differential_equations) is required to solve the ordinary differential initial value problem and allow us to see how the states change through time.
# To setup the numerical values and integrate the equations of motion we will need:
# * numpy
# * ODE numerical integration from scipy
# * PyDy's ODE function generator to transform the symbolic equations into numerical functions
# In[18]:
#get_ipython().magic('matplotlib inline')
import numpy as np
from scipy.integrate import odeint
from pydy.codegen.ode_function_generators import generate_ode_function
from matplotlib import pyplot as plt
plt.rcParams['figure.figsize'] = (14.0, 6.0)
# In[19]:
constants = [L, m, g]
specified = [thrust]
constants, coordinates, speeds, specified
# In[20]:
right_hand_side = generate_ode_function(forcing_vector, coordinates,
speeds, constants,
mass_matrix=mass_matrix,
specifieds=specified,
constants_arg_type='dictionary',
specifieds_arg_type='dictionary'
)
# In[21]:
# Set initial conditions, parameter values and time array.
x0 = np.zeros(len(coordinates) + len(speeds))
numerical_constants = {L:1.0, # Length m,
m:1.0, # Mass kg,
g:9.81}# acceleration due to gravity m/s**2
numerical_specified = {thrust:np.zeros(1)}
# In[22]:
args = {'constants': numerical_constants,
'specified': numerical_specified}
frames_per_sec = 60
final_time = 5.0
t = np.linspace(0.0, final_time, int(final_time * frames_per_sec))
# In[23]:
#help(right_hand_side)
# In[24]:
#get_ipython().magic('pdb')
right_hand_side(x0, 0.0, numerical_specified, numerical_constants)
# In[28]:
# ## Linearization
# For linearization, operating points can be specified on the call, or be substituted in afterwards. In this case we’ll provide them in the call, supplied in a list. The `A_and_B=True` kwarg indicates to solve invert the $\bf{M}$ matrix and solve for just the explicit linearized $\bf{A}$ and $\bf{B}$ matrices. The `simplify=True` kwarg indicates to simplify inside the linearize call, and return the presimplified matrices. The cost of doing this is small for simple systems, but for larger systems this can be a costly operation, and should be avoided.
# In[ ]:
# Set the operating point to be straight down, and non-moving
q_op = {qx: L, qy: 0}
u_op = {ux: 0, uy: 0}
ud_op = {uxd: 0, uyd: 0}
# Perform the linearization
A, B, inp_vec = kane.linearize(op_point=[q_op, u_op, ud_op], A_and_B=True,
new_method=True, simplify=True)
A
# In[ ]:
B
# The resulting $\bf{A}$ matrix has dimensions 2 x 2, while the number of total states is `len(q) + len(u)` = 2 + 2 = 4. This is because for constrained systems the resulting `A_and_B` form has a partitioned state vector only containing the independent coordinates and speeds. Written out mathematically, the system linearized about this point would be written as:
# $$
# \begin{bmatrix}\dot{q}_{2}\\ \dot{u}_{2}\end{bmatrix} = \begin{bmatrix}0 & 1\\ \frac{-g}{L} & 0 \end{bmatrix}\begin{bmatrix}q_{2}\\ u_{2}\end{bmatrix}
# $$
#
# In[ ]:
sym.trigsimp(fr + frstar)
# In[ ]:
B
# In[ ]:
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment