280 lines
8.3 KiB
Python
280 lines
8.3 KiB
Python
|
from sympy.physics.mechanics import (Body, Lagrangian, KanesMethod, LagrangesMethod,
|
||
|
|
RigidBody, Particle)
|
||
|
|
from sympy.physics.mechanics.method import _Methods
|
||
|
|
from sympy.core.backend import Matrix
|
||
|
|
|
||
|
|
__all__ = ['JointsMethod']
|
||
|
|
|
||
|
|
|
||
|
|
class JointsMethod(_Methods):
|
||
|
|
"""Method for formulating the equations of motion using a set of interconnected bodies with joints.
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
==========
|
||
|
|
|
||
|
|
newtonion : Body or ReferenceFrame
|
||
|
|
The newtonion(inertial) frame.
|
||
|
|
*joints : Joint
|
||
|
|
The joints in the system
|
||
|
|
|
||
|
|
Attributes
|
||
|
|
==========
|
||
|
|
|
||
|
|
q, u : iterable
|
||
|
|
Iterable of the generalized coordinates and speeds
|
||
|
|
bodies : iterable
|
||
|
|
Iterable of Body objects in the system.
|
||
|
|
loads : iterable
|
||
|
|
Iterable of (Point, vector) or (ReferenceFrame, vector) tuples
|
||
|
|
describing the forces on the system.
|
||
|
|
mass_matrix : Matrix, shape(n, n)
|
||
|
|
The system's mass matrix
|
||
|
|
forcing : Matrix, shape(n, 1)
|
||
|
|
The system's forcing vector
|
||
|
|
mass_matrix_full : Matrix, shape(2*n, 2*n)
|
||
|
|
The "mass matrix" for the u's and q's
|
||
|
|
forcing_full : Matrix, shape(2*n, 1)
|
||
|
|
The "forcing vector" for the u's and q's
|
||
|
|
method : KanesMethod or Lagrange's method
|
||
|
|
Method's object.
|
||
|
|
kdes : iterable
|
||
|
|
Iterable of kde in they system.
|
||
|
|
|
||
|
|
Examples
|
||
|
|
========
|
||
|
|
|
||
|
|
This is a simple example for a one degree of freedom translational
|
||
|
|
spring-mass-damper.
|
||
|
|
|
||
|
|
>>> from sympy import symbols
|
||
|
|
>>> from sympy.physics.mechanics import Body, JointsMethod, PrismaticJoint
|
||
|
|
>>> from sympy.physics.vector import dynamicsymbols
|
||
|
|
>>> c, k = symbols('c k')
|
||
|
|
>>> x, v = dynamicsymbols('x v')
|
||
|
|
>>> wall = Body('W')
|
||
|
|
>>> body = Body('B')
|
||
|
|
>>> J = PrismaticJoint('J', wall, body, coordinates=x, speeds=v)
|
||
|
|
>>> wall.apply_force(c*v*wall.x, reaction_body=body)
|
||
|
|
>>> wall.apply_force(k*x*wall.x, reaction_body=body)
|
||
|
|
>>> method = JointsMethod(wall, J)
|
||
|
|
>>> method.form_eoms()
|
||
|
|
Matrix([[-B_mass*Derivative(v(t), t) - c*v(t) - k*x(t)]])
|
||
|
|
>>> M = method.mass_matrix_full
|
||
|
|
>>> F = method.forcing_full
|
||
|
|
>>> rhs = M.LUsolve(F)
|
||
|
|
>>> rhs
|
||
|
|
Matrix([
|
||
|
|
[ v(t)],
|
||
|
|
[(-c*v(t) - k*x(t))/B_mass]])
|
||
|
|
|
||
|
|
Notes
|
||
|
|
=====
|
||
|
|
|
||
|
|
``JointsMethod`` currently only works with systems that do not have any
|
||
|
|
configuration or motion constraints.
|
||
|
|
|
||
|
|
"""
|
||
|
|
|
||
|
|
def __init__(self, newtonion, *joints):
|
||
|
|
if isinstance(newtonion, Body):
|
||
|
|
self.frame = newtonion.frame
|
||
|
|
else:
|
||
|
|
self.frame = newtonion
|
||
|
|
|
||
|
|
self._joints = joints
|
||
|
|
self._bodies = self._generate_bodylist()
|
||
|
|
self._loads = self._generate_loadlist()
|
||
|
|
self._q = self._generate_q()
|
||
|
|
self._u = self._generate_u()
|
||
|
|
self._kdes = self._generate_kdes()
|
||
|
|
|
||
|
|
self._method = None
|
||
|
|
|
||
|
|
@property
|
||
|
|
def bodies(self):
|
||
|
|
"""List of bodies in they system."""
|
||
|
|
return self._bodies
|
||
|
|
|
||
|
|
@property
|
||
|
|
def loads(self):
|
||
|
|
"""List of loads on the system."""
|
||
|
|
return self._loads
|
||
|
|
|
||
|
|
@property
|
||
|
|
def q(self):
|
||
|
|
"""List of the generalized coordinates."""
|
||
|
|
return self._q
|
||
|
|
|
||
|
|
@property
|
||
|
|
def u(self):
|
||
|
|
"""List of the generalized speeds."""
|
||
|
|
return self._u
|
||
|
|
|
||
|
|
@property
|
||
|
|
def kdes(self):
|
||
|
|
"""List of the generalized coordinates."""
|
||
|
|
return self._kdes
|
||
|
|
|
||
|
|
@property
|
||
|
|
def forcing_full(self):
|
||
|
|
"""The "forcing vector" for the u's and q's."""
|
||
|
|
return self.method.forcing_full
|
||
|
|
|
||
|
|
@property
|
||
|
|
def mass_matrix_full(self):
|
||
|
|
"""The "mass matrix" for the u's and q's."""
|
||
|
|
return self.method.mass_matrix_full
|
||
|
|
|
||
|
|
@property
|
||
|
|
def mass_matrix(self):
|
||
|
|
"""The system's mass matrix."""
|
||
|
|
return self.method.mass_matrix
|
||
|
|
|
||
|
|
@property
|
||
|
|
def forcing(self):
|
||
|
|
"""The system's forcing vector."""
|
||
|
|
return self.method.forcing
|
||
|
|
|
||
|
|
@property
|
||
|
|
def method(self):
|
||
|
|
"""Object of method used to form equations of systems."""
|
||
|
|
return self._method
|
||
|
|
|
||
|
|
def _generate_bodylist(self):
|
||
|
|
bodies = []
|
||
|
|
for joint in self._joints:
|
||
|
|
if joint.child not in bodies:
|
||
|
|
bodies.append(joint.child)
|
||
|
|
if joint.parent not in bodies:
|
||
|
|
bodies.append(joint.parent)
|
||
|
|
return bodies
|
||
|
|
|
||
|
|
def _generate_loadlist(self):
|
||
|
|
load_list = []
|
||
|
|
for body in self.bodies:
|
||
|
|
load_list.extend(body.loads)
|
||
|
|
return load_list
|
||
|
|
|
||
|
|
def _generate_q(self):
|
||
|
|
q_ind = []
|
||
|
|
for joint in self._joints:
|
||
|
|
for coordinate in joint.coordinates:
|
||
|
|
if coordinate in q_ind:
|
||
|
|
raise ValueError('Coordinates of joints should be unique.')
|
||
|
|
q_ind.append(coordinate)
|
||
|
|
return Matrix(q_ind)
|
||
|
|
|
||
|
|
def _generate_u(self):
|
||
|
|
u_ind = []
|
||
|
|
for joint in self._joints:
|
||
|
|
for speed in joint.speeds:
|
||
|
|
if speed in u_ind:
|
||
|
|
raise ValueError('Speeds of joints should be unique.')
|
||
|
|
u_ind.append(speed)
|
||
|
|
return Matrix(u_ind)
|
||
|
|
|
||
|
|
def _generate_kdes(self):
|
||
|
|
kd_ind = Matrix(1, 0, []).T
|
||
|
|
for joint in self._joints:
|
||
|
|
kd_ind = kd_ind.col_join(joint.kdes)
|
||
|
|
return kd_ind
|
||
|
|
|
||
|
|
def _convert_bodies(self):
|
||
|
|
# Convert `Body` to `Particle` and `RigidBody`
|
||
|
|
bodylist = []
|
||
|
|
for body in self.bodies:
|
||
|
|
if body.is_rigidbody:
|
||
|
|
rb = RigidBody(body.name, body.masscenter, body.frame, body.mass,
|
||
|
|
(body.central_inertia, body.masscenter))
|
||
|
|
rb.potential_energy = body.potential_energy
|
||
|
|
bodylist.append(rb)
|
||
|
|
else:
|
||
|
|
part = Particle(body.name, body.masscenter, body.mass)
|
||
|
|
part.potential_energy = body.potential_energy
|
||
|
|
bodylist.append(part)
|
||
|
|
return bodylist
|
||
|
|
|
||
|
|
def form_eoms(self, method=KanesMethod):
|
||
|
|
"""Method to form system's equation of motions.
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
==========
|
||
|
|
|
||
|
|
method : Class
|
||
|
|
Class name of method.
|
||
|
|
|
||
|
|
Returns
|
||
|
|
========
|
||
|
|
|
||
|
|
Matrix
|
||
|
|
Vector of equations of motions.
|
||
|
|
|
||
|
|
Examples
|
||
|
|
========
|
||
|
|
|
||
|
|
This is a simple example for a one degree of freedom translational
|
||
|
|
spring-mass-damper.
|
||
|
|
|
||
|
|
>>> from sympy import S, symbols
|
||
|
|
>>> from sympy.physics.mechanics import LagrangesMethod, dynamicsymbols, Body
|
||
|
|
>>> from sympy.physics.mechanics import PrismaticJoint, JointsMethod
|
||
|
|
>>> q = dynamicsymbols('q')
|
||
|
|
>>> qd = dynamicsymbols('q', 1)
|
||
|
|
>>> m, k, b = symbols('m k b')
|
||
|
|
>>> wall = Body('W')
|
||
|
|
>>> part = Body('P', mass=m)
|
||
|
|
>>> part.potential_energy = k * q**2 / S(2)
|
||
|
|
>>> J = PrismaticJoint('J', wall, part, coordinates=q, speeds=qd)
|
||
|
|
>>> wall.apply_force(b * qd * wall.x, reaction_body=part)
|
||
|
|
>>> method = JointsMethod(wall, J)
|
||
|
|
>>> method.form_eoms(LagrangesMethod)
|
||
|
|
Matrix([[b*Derivative(q(t), t) + k*q(t) + m*Derivative(q(t), (t, 2))]])
|
||
|
|
|
||
|
|
We can also solve for the states using the 'rhs' method.
|
||
|
|
|
||
|
|
>>> method.rhs()
|
||
|
|
Matrix([
|
||
|
|
[ Derivative(q(t), t)],
|
||
|
|
[(-b*Derivative(q(t), t) - k*q(t))/m]])
|
||
|
|
|
||
|
|
"""
|
||
|
|
|
||
|
|
bodylist = self._convert_bodies()
|
||
|
|
if issubclass(method, LagrangesMethod): #LagrangesMethod or similar
|
||
|
|
L = Lagrangian(self.frame, *bodylist)
|
||
|
|
self._method = method(L, self.q, self.loads, bodylist, self.frame)
|
||
|
|
else: #KanesMethod or similar
|
||
|
|
self._method = method(self.frame, q_ind=self.q, u_ind=self.u, kd_eqs=self.kdes,
|
||
|
|
forcelist=self.loads, bodies=bodylist)
|
||
|
|
soln = self.method._form_eoms()
|
||
|
|
return soln
|
||
|
|
|
||
|
|
def rhs(self, inv_method=None):
|
||
|
|
"""Returns equations that can be solved numerically.
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
==========
|
||
|
|
|
||
|
|
inv_method : str
|
||
|
|
The specific sympy inverse matrix calculation method to use. For a
|
||
|
|
list of valid methods, see
|
||
|
|
:meth:`~sympy.matrices.matrices.MatrixBase.inv`
|
||
|
|
|
||
|
|
Returns
|
||
|
|
========
|
||
|
|
|
||
|
|
Matrix
|
||
|
|
Numerically solvable equations.
|
||
|
|
|
||
|
|
See Also
|
||
|
|
========
|
||
|
|
|
||
|
|
sympy.physics.mechanics.kane.KanesMethod.rhs:
|
||
|
|
KanesMethod's rhs function.
|
||
|
|
sympy.physics.mechanics.lagrange.LagrangesMethod.rhs:
|
||
|
|
LagrangesMethod's rhs function.
|
||
|
|
|
||
|
|
"""
|
||
|
|
|
||
|
|
return self.method.rhs(inv_method=inv_method)
|