AI Newsletter Digest improvements: fixed QP soft line break decoding, URL extraction, and content cleaning
This commit is contained in:
+90
@@ -0,0 +1,90 @@
|
||||
__all__ = [
|
||||
'vector',
|
||||
|
||||
'CoordinateSym', 'ReferenceFrame', 'Dyadic', 'Vector', 'Point', 'cross',
|
||||
'dot', 'express', 'time_derivative', 'outer', 'kinematic_equations',
|
||||
'get_motion_params', 'partial_velocity', 'dynamicsymbols', 'vprint',
|
||||
'vsstrrepr', 'vsprint', 'vpprint', 'vlatex', 'init_vprinting', 'curl',
|
||||
'divergence', 'gradient', 'is_conservative', 'is_solenoidal',
|
||||
'scalar_potential', 'scalar_potential_difference',
|
||||
|
||||
'KanesMethod',
|
||||
|
||||
'RigidBody',
|
||||
|
||||
'linear_momentum', 'angular_momentum', 'kinetic_energy', 'potential_energy',
|
||||
'Lagrangian', 'mechanics_printing', 'mprint', 'msprint', 'mpprint',
|
||||
'mlatex', 'msubs', 'find_dynamicsymbols',
|
||||
|
||||
'inertia', 'inertia_of_point_mass', 'Inertia',
|
||||
|
||||
'Force', 'Torque',
|
||||
|
||||
'Particle',
|
||||
|
||||
'LagrangesMethod',
|
||||
|
||||
'Linearizer',
|
||||
|
||||
'Body',
|
||||
|
||||
'SymbolicSystem', 'System',
|
||||
|
||||
'PinJoint', 'PrismaticJoint', 'CylindricalJoint', 'PlanarJoint',
|
||||
'SphericalJoint', 'WeldJoint',
|
||||
|
||||
'JointsMethod',
|
||||
|
||||
'WrappingCylinder', 'WrappingGeometryBase', 'WrappingSphere',
|
||||
|
||||
'PathwayBase', 'LinearPathway', 'ObstacleSetPathway', 'WrappingPathway',
|
||||
|
||||
'ActuatorBase', 'ForceActuator', 'LinearDamper', 'LinearSpring',
|
||||
'TorqueActuator', 'DuffingSpring', 'CoulombKineticFriction',
|
||||
]
|
||||
|
||||
from sympy.physics import vector
|
||||
|
||||
from sympy.physics.vector import (CoordinateSym, ReferenceFrame, Dyadic, Vector, Point,
|
||||
cross, dot, express, time_derivative, outer, kinematic_equations,
|
||||
get_motion_params, partial_velocity, dynamicsymbols, vprint,
|
||||
vsstrrepr, vsprint, vpprint, vlatex, init_vprinting, curl, divergence,
|
||||
gradient, is_conservative, is_solenoidal, scalar_potential,
|
||||
scalar_potential_difference)
|
||||
|
||||
from .kane import KanesMethod
|
||||
|
||||
from .rigidbody import RigidBody
|
||||
|
||||
from .functions import (linear_momentum, angular_momentum, kinetic_energy,
|
||||
potential_energy, Lagrangian, mechanics_printing,
|
||||
mprint, msprint, mpprint, mlatex, msubs,
|
||||
find_dynamicsymbols)
|
||||
|
||||
from .inertia import inertia, inertia_of_point_mass, Inertia
|
||||
|
||||
from .loads import Force, Torque
|
||||
|
||||
from .particle import Particle
|
||||
|
||||
from .lagrange import LagrangesMethod
|
||||
|
||||
from .linearize import Linearizer
|
||||
|
||||
from .body import Body
|
||||
|
||||
from .system import SymbolicSystem, System
|
||||
|
||||
from .jointsmethod import JointsMethod
|
||||
|
||||
from .joint import (PinJoint, PrismaticJoint, CylindricalJoint, PlanarJoint,
|
||||
SphericalJoint, WeldJoint)
|
||||
|
||||
from .wrapping_geometry import (WrappingCylinder, WrappingGeometryBase,
|
||||
WrappingSphere)
|
||||
|
||||
from .pathway import (PathwayBase, LinearPathway, ObstacleSetPathway,
|
||||
WrappingPathway)
|
||||
|
||||
from .actuator import (ActuatorBase, ForceActuator, LinearDamper, LinearSpring,
|
||||
TorqueActuator, DuffingSpring, CoulombKineticFriction)
|
||||
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
+1147
File diff suppressed because it is too large
Load Diff
+710
@@ -0,0 +1,710 @@
|
||||
from sympy import Symbol
|
||||
from sympy.physics.vector import Point, Vector, ReferenceFrame, Dyadic
|
||||
from sympy.physics.mechanics import RigidBody, Particle, Inertia
|
||||
from sympy.physics.mechanics.body_base import BodyBase
|
||||
from sympy.utilities.exceptions import sympy_deprecation_warning
|
||||
|
||||
__all__ = ['Body']
|
||||
|
||||
|
||||
# XXX: We use type:ignore because the classes RigidBody and Particle have
|
||||
# inconsistent parallel axis methods that take different numbers of arguments.
|
||||
class Body(RigidBody, Particle): # type: ignore
|
||||
"""
|
||||
Body is a common representation of either a RigidBody or a Particle SymPy
|
||||
object depending on what is passed in during initialization. If a mass is
|
||||
passed in and central_inertia is left as None, the Particle object is
|
||||
created. Otherwise a RigidBody object will be created.
|
||||
|
||||
.. deprecated:: 1.13
|
||||
The Body class is deprecated. Its functionality is captured by
|
||||
:class:`~.RigidBody` and :class:`~.Particle`.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
The attributes that Body possesses will be the same as a Particle instance
|
||||
or a Rigid Body instance depending on which was created. Additional
|
||||
attributes are listed below.
|
||||
|
||||
Attributes
|
||||
==========
|
||||
|
||||
name : string
|
||||
The body's name
|
||||
masscenter : Point
|
||||
The point which represents the center of mass of the rigid body
|
||||
frame : ReferenceFrame
|
||||
The reference frame which the body is fixed in
|
||||
mass : Sympifyable
|
||||
The body's mass
|
||||
inertia : (Dyadic, Point)
|
||||
The body's inertia around its center of mass. This attribute is specific
|
||||
to the rigid body form of Body and is left undefined for the Particle
|
||||
form
|
||||
loads : iterable
|
||||
This list contains information on the different loads acting on the
|
||||
Body. Forces are listed as a (point, vector) tuple and torques are
|
||||
listed as (reference frame, vector) tuples.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
name : String
|
||||
Defines the name of the body. It is used as the base for defining
|
||||
body specific properties.
|
||||
masscenter : Point, optional
|
||||
A point that represents the center of mass of the body or particle.
|
||||
If no point is given, a point is generated.
|
||||
mass : Sympifyable, optional
|
||||
A Sympifyable object which represents the mass of the body. If no
|
||||
mass is passed, one is generated.
|
||||
frame : ReferenceFrame, optional
|
||||
The ReferenceFrame that represents the reference frame of the body.
|
||||
If no frame is given, a frame is generated.
|
||||
central_inertia : Dyadic, optional
|
||||
Central inertia dyadic of the body. If none is passed while creating
|
||||
RigidBody, a default inertia is generated.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
As Body has been deprecated, the following examples are for illustrative
|
||||
purposes only. The functionality of Body is fully captured by
|
||||
:class:`~.RigidBody` and :class:`~.Particle`. To ignore the deprecation
|
||||
warning we can use the ignore_warnings context manager.
|
||||
|
||||
>>> from sympy.utilities.exceptions import ignore_warnings
|
||||
|
||||
Default behaviour. This results in the creation of a RigidBody object for
|
||||
which the mass, mass center, frame and inertia attributes are given default
|
||||
values. ::
|
||||
|
||||
>>> from sympy.physics.mechanics import Body
|
||||
>>> with ignore_warnings(DeprecationWarning):
|
||||
... body = Body('name_of_body')
|
||||
|
||||
This next example demonstrates the code required to specify all of the
|
||||
values of the Body object. Note this will also create a RigidBody version of
|
||||
the Body object. ::
|
||||
|
||||
>>> from sympy import Symbol
|
||||
>>> from sympy.physics.mechanics import ReferenceFrame, Point, inertia
|
||||
>>> from sympy.physics.mechanics import Body
|
||||
>>> mass = Symbol('mass')
|
||||
>>> masscenter = Point('masscenter')
|
||||
>>> frame = ReferenceFrame('frame')
|
||||
>>> ixx = Symbol('ixx')
|
||||
>>> body_inertia = inertia(frame, ixx, 0, 0)
|
||||
>>> with ignore_warnings(DeprecationWarning):
|
||||
... body = Body('name_of_body', masscenter, mass, frame, body_inertia)
|
||||
|
||||
The minimal code required to create a Particle version of the Body object
|
||||
involves simply passing in a name and a mass. ::
|
||||
|
||||
>>> from sympy import Symbol
|
||||
>>> from sympy.physics.mechanics import Body
|
||||
>>> mass = Symbol('mass')
|
||||
>>> with ignore_warnings(DeprecationWarning):
|
||||
... body = Body('name_of_body', mass=mass)
|
||||
|
||||
The Particle version of the Body object can also receive a masscenter point
|
||||
and a reference frame, just not an inertia.
|
||||
"""
|
||||
|
||||
def __init__(self, name, masscenter=None, mass=None, frame=None,
|
||||
central_inertia=None):
|
||||
sympy_deprecation_warning(
|
||||
"""
|
||||
Support for the Body class has been removed, as its functionality is
|
||||
fully captured by RigidBody and Particle.
|
||||
""",
|
||||
deprecated_since_version="1.13",
|
||||
active_deprecations_target="deprecated-mechanics-body-class"
|
||||
)
|
||||
|
||||
self._loads = []
|
||||
|
||||
if frame is None:
|
||||
frame = ReferenceFrame(name + '_frame')
|
||||
|
||||
if masscenter is None:
|
||||
masscenter = Point(name + '_masscenter')
|
||||
|
||||
if central_inertia is None and mass is None:
|
||||
ixx = Symbol(name + '_ixx')
|
||||
iyy = Symbol(name + '_iyy')
|
||||
izz = Symbol(name + '_izz')
|
||||
izx = Symbol(name + '_izx')
|
||||
ixy = Symbol(name + '_ixy')
|
||||
iyz = Symbol(name + '_iyz')
|
||||
_inertia = Inertia.from_inertia_scalars(masscenter, frame, ixx, iyy,
|
||||
izz, ixy, iyz, izx)
|
||||
else:
|
||||
_inertia = (central_inertia, masscenter)
|
||||
|
||||
if mass is None:
|
||||
_mass = Symbol(name + '_mass')
|
||||
else:
|
||||
_mass = mass
|
||||
|
||||
masscenter.set_vel(frame, 0)
|
||||
|
||||
# If user passes masscenter and mass then a particle is created
|
||||
# otherwise a rigidbody. As a result a body may or may not have inertia.
|
||||
# Note: BodyBase.__init__ is used to prevent problems with super() calls in
|
||||
# Particle and RigidBody arising due to multiple inheritance.
|
||||
if central_inertia is None and mass is not None:
|
||||
BodyBase.__init__(self, name, masscenter, _mass)
|
||||
self.frame = frame
|
||||
self._central_inertia = Dyadic(0)
|
||||
else:
|
||||
BodyBase.__init__(self, name, masscenter, _mass)
|
||||
self.frame = frame
|
||||
self.inertia = _inertia
|
||||
|
||||
def __repr__(self):
|
||||
if self.is_rigidbody:
|
||||
return RigidBody.__repr__(self)
|
||||
return Particle.__repr__(self)
|
||||
|
||||
@property
|
||||
def loads(self):
|
||||
return self._loads
|
||||
|
||||
@property
|
||||
def x(self):
|
||||
"""The basis Vector for the Body, in the x direction."""
|
||||
return self.frame.x
|
||||
|
||||
@property
|
||||
def y(self):
|
||||
"""The basis Vector for the Body, in the y direction."""
|
||||
return self.frame.y
|
||||
|
||||
@property
|
||||
def z(self):
|
||||
"""The basis Vector for the Body, in the z direction."""
|
||||
return self.frame.z
|
||||
|
||||
@property
|
||||
def inertia(self):
|
||||
"""The body's inertia about a point; stored as (Dyadic, Point)."""
|
||||
if self.is_rigidbody:
|
||||
return RigidBody.inertia.fget(self)
|
||||
return (self.central_inertia, self.masscenter)
|
||||
|
||||
@inertia.setter
|
||||
def inertia(self, I):
|
||||
RigidBody.inertia.fset(self, I)
|
||||
|
||||
@property
|
||||
def is_rigidbody(self):
|
||||
if hasattr(self, '_inertia'):
|
||||
return True
|
||||
return False
|
||||
|
||||
def kinetic_energy(self, frame):
|
||||
"""Kinetic energy of the body.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
frame : ReferenceFrame or Body
|
||||
The Body's angular velocity and the velocity of it's mass
|
||||
center are typically defined with respect to an inertial frame but
|
||||
any relevant frame in which the velocities are known can be supplied.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
As Body has been deprecated, the following examples are for illustrative
|
||||
purposes only. The functionality of Body is fully captured by
|
||||
:class:`~.RigidBody` and :class:`~.Particle`. To ignore the deprecation
|
||||
warning we can use the ignore_warnings context manager.
|
||||
|
||||
>>> from sympy.utilities.exceptions import ignore_warnings
|
||||
>>> from sympy.physics.mechanics import Body, ReferenceFrame, Point
|
||||
>>> from sympy import symbols
|
||||
>>> m, v, r, omega = symbols('m v r omega')
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> O = Point('O')
|
||||
>>> with ignore_warnings(DeprecationWarning):
|
||||
... P = Body('P', masscenter=O, mass=m)
|
||||
>>> P.masscenter.set_vel(N, v * N.y)
|
||||
>>> P.kinetic_energy(N)
|
||||
m*v**2/2
|
||||
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> b = ReferenceFrame('b')
|
||||
>>> b.set_ang_vel(N, omega * b.x)
|
||||
>>> P = Point('P')
|
||||
>>> P.set_vel(N, v * N.x)
|
||||
>>> with ignore_warnings(DeprecationWarning):
|
||||
... B = Body('B', masscenter=P, frame=b)
|
||||
>>> B.kinetic_energy(N)
|
||||
B_ixx*omega**2/2 + B_mass*v**2/2
|
||||
|
||||
See Also
|
||||
========
|
||||
|
||||
sympy.physics.mechanics : Particle, RigidBody
|
||||
|
||||
"""
|
||||
if isinstance(frame, Body):
|
||||
frame = Body.frame
|
||||
if self.is_rigidbody:
|
||||
return RigidBody(self.name, self.masscenter, self.frame, self.mass,
|
||||
(self.central_inertia, self.masscenter)).kinetic_energy(frame)
|
||||
return Particle(self.name, self.masscenter, self.mass).kinetic_energy(frame)
|
||||
|
||||
def apply_force(self, force, point=None, reaction_body=None, reaction_point=None):
|
||||
"""Add force to the body(s).
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
Applies the force on self or equal and opposite forces on
|
||||
self and other body if both are given on the desired point on the bodies.
|
||||
The force applied on other body is taken opposite of self, i.e, -force.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
force: Vector
|
||||
The force to be applied.
|
||||
point: Point, optional
|
||||
The point on self on which force is applied.
|
||||
By default self's masscenter.
|
||||
reaction_body: Body, optional
|
||||
Second body on which equal and opposite force
|
||||
is to be applied.
|
||||
reaction_point : Point, optional
|
||||
The point on other body on which equal and opposite
|
||||
force is applied. By default masscenter of other body.
|
||||
|
||||
Example
|
||||
=======
|
||||
|
||||
As Body has been deprecated, the following examples are for illustrative
|
||||
purposes only. The functionality of Body is fully captured by
|
||||
:class:`~.RigidBody` and :class:`~.Particle`. To ignore the deprecation
|
||||
warning we can use the ignore_warnings context manager.
|
||||
|
||||
>>> from sympy.utilities.exceptions import ignore_warnings
|
||||
>>> from sympy import symbols
|
||||
>>> from sympy.physics.mechanics import Body, Point, dynamicsymbols
|
||||
>>> m, g = symbols('m g')
|
||||
>>> with ignore_warnings(DeprecationWarning):
|
||||
... B = Body('B')
|
||||
>>> force1 = m*g*B.z
|
||||
>>> B.apply_force(force1) #Applying force on B's masscenter
|
||||
>>> B.loads
|
||||
[(B_masscenter, g*m*B_frame.z)]
|
||||
|
||||
We can also remove some part of force from any point on the body by
|
||||
adding the opposite force to the body on that point.
|
||||
|
||||
>>> f1, f2 = dynamicsymbols('f1 f2')
|
||||
>>> P = Point('P') #Considering point P on body B
|
||||
>>> B.apply_force(f1*B.x + f2*B.y, P)
|
||||
>>> B.loads
|
||||
[(B_masscenter, g*m*B_frame.z), (P, f1(t)*B_frame.x + f2(t)*B_frame.y)]
|
||||
|
||||
Let's remove f1 from point P on body B.
|
||||
|
||||
>>> B.apply_force(-f1*B.x, P)
|
||||
>>> B.loads
|
||||
[(B_masscenter, g*m*B_frame.z), (P, f2(t)*B_frame.y)]
|
||||
|
||||
To further demonstrate the use of ``apply_force`` attribute,
|
||||
consider two bodies connected through a spring.
|
||||
|
||||
>>> from sympy.physics.mechanics import Body, dynamicsymbols
|
||||
>>> with ignore_warnings(DeprecationWarning):
|
||||
... N = Body('N') #Newtonion Frame
|
||||
>>> x = dynamicsymbols('x')
|
||||
>>> with ignore_warnings(DeprecationWarning):
|
||||
... B1 = Body('B1')
|
||||
... B2 = Body('B2')
|
||||
>>> spring_force = x*N.x
|
||||
|
||||
Now let's apply equal and opposite spring force to the bodies.
|
||||
|
||||
>>> P1 = Point('P1')
|
||||
>>> P2 = Point('P2')
|
||||
>>> B1.apply_force(spring_force, point=P1, reaction_body=B2, reaction_point=P2)
|
||||
|
||||
We can check the loads(forces) applied to bodies now.
|
||||
|
||||
>>> B1.loads
|
||||
[(P1, x(t)*N_frame.x)]
|
||||
>>> B2.loads
|
||||
[(P2, - x(t)*N_frame.x)]
|
||||
|
||||
Notes
|
||||
=====
|
||||
|
||||
If a new force is applied to a body on a point which already has some
|
||||
force applied on it, then the new force is added to the already applied
|
||||
force on that point.
|
||||
|
||||
"""
|
||||
|
||||
if not isinstance(point, Point):
|
||||
if point is None:
|
||||
point = self.masscenter # masscenter
|
||||
else:
|
||||
raise TypeError("Force must be applied to a point on the body.")
|
||||
if not isinstance(force, Vector):
|
||||
raise TypeError("Force must be a vector.")
|
||||
|
||||
if reaction_body is not None:
|
||||
reaction_body.apply_force(-force, point=reaction_point)
|
||||
|
||||
for load in self._loads:
|
||||
if point in load:
|
||||
force += load[1]
|
||||
self._loads.remove(load)
|
||||
break
|
||||
|
||||
self._loads.append((point, force))
|
||||
|
||||
def apply_torque(self, torque, reaction_body=None):
|
||||
"""Add torque to the body(s).
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
Applies the torque on self or equal and opposite torques on
|
||||
self and other body if both are given.
|
||||
The torque applied on other body is taken opposite of self,
|
||||
i.e, -torque.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
torque: Vector
|
||||
The torque to be applied.
|
||||
reaction_body: Body, optional
|
||||
Second body on which equal and opposite torque
|
||||
is to be applied.
|
||||
|
||||
Example
|
||||
=======
|
||||
|
||||
As Body has been deprecated, the following examples are for illustrative
|
||||
purposes only. The functionality of Body is fully captured by
|
||||
:class:`~.RigidBody` and :class:`~.Particle`. To ignore the deprecation
|
||||
warning we can use the ignore_warnings context manager.
|
||||
|
||||
>>> from sympy.utilities.exceptions import ignore_warnings
|
||||
>>> from sympy import symbols
|
||||
>>> from sympy.physics.mechanics import Body, dynamicsymbols
|
||||
>>> t = symbols('t')
|
||||
>>> with ignore_warnings(DeprecationWarning):
|
||||
... B = Body('B')
|
||||
>>> torque1 = t*B.z
|
||||
>>> B.apply_torque(torque1)
|
||||
>>> B.loads
|
||||
[(B_frame, t*B_frame.z)]
|
||||
|
||||
We can also remove some part of torque from the body by
|
||||
adding the opposite torque to the body.
|
||||
|
||||
>>> t1, t2 = dynamicsymbols('t1 t2')
|
||||
>>> B.apply_torque(t1*B.x + t2*B.y)
|
||||
>>> B.loads
|
||||
[(B_frame, t1(t)*B_frame.x + t2(t)*B_frame.y + t*B_frame.z)]
|
||||
|
||||
Let's remove t1 from Body B.
|
||||
|
||||
>>> B.apply_torque(-t1*B.x)
|
||||
>>> B.loads
|
||||
[(B_frame, t2(t)*B_frame.y + t*B_frame.z)]
|
||||
|
||||
To further demonstrate the use, let us consider two bodies such that
|
||||
a torque `T` is acting on one body, and `-T` on the other.
|
||||
|
||||
>>> from sympy.physics.mechanics import Body, dynamicsymbols
|
||||
>>> with ignore_warnings(DeprecationWarning):
|
||||
... N = Body('N') #Newtonion frame
|
||||
... B1 = Body('B1')
|
||||
... B2 = Body('B2')
|
||||
>>> v = dynamicsymbols('v')
|
||||
>>> T = v*N.y #Torque
|
||||
|
||||
Now let's apply equal and opposite torque to the bodies.
|
||||
|
||||
>>> B1.apply_torque(T, B2)
|
||||
|
||||
We can check the loads (torques) applied to bodies now.
|
||||
|
||||
>>> B1.loads
|
||||
[(B1_frame, v(t)*N_frame.y)]
|
||||
>>> B2.loads
|
||||
[(B2_frame, - v(t)*N_frame.y)]
|
||||
|
||||
Notes
|
||||
=====
|
||||
|
||||
If a new torque is applied on body which already has some torque applied on it,
|
||||
then the new torque is added to the previous torque about the body's frame.
|
||||
|
||||
"""
|
||||
|
||||
if not isinstance(torque, Vector):
|
||||
raise TypeError("A Vector must be supplied to add torque.")
|
||||
|
||||
if reaction_body is not None:
|
||||
reaction_body.apply_torque(-torque)
|
||||
|
||||
for load in self._loads:
|
||||
if self.frame in load:
|
||||
torque += load[1]
|
||||
self._loads.remove(load)
|
||||
break
|
||||
self._loads.append((self.frame, torque))
|
||||
|
||||
def clear_loads(self):
|
||||
"""
|
||||
Clears the Body's loads list.
|
||||
|
||||
Example
|
||||
=======
|
||||
|
||||
As Body has been deprecated, the following examples are for illustrative
|
||||
purposes only. The functionality of Body is fully captured by
|
||||
:class:`~.RigidBody` and :class:`~.Particle`. To ignore the deprecation
|
||||
warning we can use the ignore_warnings context manager.
|
||||
|
||||
>>> from sympy.utilities.exceptions import ignore_warnings
|
||||
>>> from sympy.physics.mechanics import Body
|
||||
>>> with ignore_warnings(DeprecationWarning):
|
||||
... B = Body('B')
|
||||
>>> force = B.x + B.y
|
||||
>>> B.apply_force(force)
|
||||
>>> B.loads
|
||||
[(B_masscenter, B_frame.x + B_frame.y)]
|
||||
>>> B.clear_loads()
|
||||
>>> B.loads
|
||||
[]
|
||||
|
||||
"""
|
||||
|
||||
self._loads = []
|
||||
|
||||
def remove_load(self, about=None):
|
||||
"""
|
||||
Remove load about a point or frame.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
about : Point or ReferenceFrame, optional
|
||||
The point about which force is applied,
|
||||
and is to be removed.
|
||||
If about is None, then the torque about
|
||||
self's frame is removed.
|
||||
|
||||
Example
|
||||
=======
|
||||
|
||||
As Body has been deprecated, the following examples are for illustrative
|
||||
purposes only. The functionality of Body is fully captured by
|
||||
:class:`~.RigidBody` and :class:`~.Particle`. To ignore the deprecation
|
||||
warning we can use the ignore_warnings context manager.
|
||||
|
||||
>>> from sympy.utilities.exceptions import ignore_warnings
|
||||
>>> from sympy.physics.mechanics import Body, Point
|
||||
>>> with ignore_warnings(DeprecationWarning):
|
||||
... B = Body('B')
|
||||
>>> P = Point('P')
|
||||
>>> f1 = B.x
|
||||
>>> f2 = B.y
|
||||
>>> B.apply_force(f1)
|
||||
>>> B.apply_force(f2, P)
|
||||
>>> B.loads
|
||||
[(B_masscenter, B_frame.x), (P, B_frame.y)]
|
||||
|
||||
>>> B.remove_load(P)
|
||||
>>> B.loads
|
||||
[(B_masscenter, B_frame.x)]
|
||||
|
||||
"""
|
||||
|
||||
if about is not None:
|
||||
if not isinstance(about, Point):
|
||||
raise TypeError('Load is applied about Point or ReferenceFrame.')
|
||||
else:
|
||||
about = self.frame
|
||||
|
||||
for load in self._loads:
|
||||
if about in load:
|
||||
self._loads.remove(load)
|
||||
break
|
||||
|
||||
def masscenter_vel(self, body):
|
||||
"""
|
||||
Returns the velocity of the mass center with respect to the provided
|
||||
rigid body or reference frame.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
body: Body or ReferenceFrame
|
||||
The rigid body or reference frame to calculate the velocity in.
|
||||
|
||||
Example
|
||||
=======
|
||||
|
||||
As Body has been deprecated, the following examples are for illustrative
|
||||
purposes only. The functionality of Body is fully captured by
|
||||
:class:`~.RigidBody` and :class:`~.Particle`. To ignore the deprecation
|
||||
warning we can use the ignore_warnings context manager.
|
||||
|
||||
>>> from sympy.utilities.exceptions import ignore_warnings
|
||||
>>> from sympy.physics.mechanics import Body
|
||||
>>> with ignore_warnings(DeprecationWarning):
|
||||
... A = Body('A')
|
||||
... B = Body('B')
|
||||
>>> A.masscenter.set_vel(B.frame, 5*B.frame.x)
|
||||
>>> A.masscenter_vel(B)
|
||||
5*B_frame.x
|
||||
>>> A.masscenter_vel(B.frame)
|
||||
5*B_frame.x
|
||||
|
||||
"""
|
||||
|
||||
if isinstance(body, ReferenceFrame):
|
||||
frame=body
|
||||
elif isinstance(body, Body):
|
||||
frame = body.frame
|
||||
return self.masscenter.vel(frame)
|
||||
|
||||
def ang_vel_in(self, body):
|
||||
"""
|
||||
Returns this body's angular velocity with respect to the provided
|
||||
rigid body or reference frame.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
body: Body or ReferenceFrame
|
||||
The rigid body or reference frame to calculate the angular velocity in.
|
||||
|
||||
Example
|
||||
=======
|
||||
|
||||
As Body has been deprecated, the following examples are for illustrative
|
||||
purposes only. The functionality of Body is fully captured by
|
||||
:class:`~.RigidBody` and :class:`~.Particle`. To ignore the deprecation
|
||||
warning we can use the ignore_warnings context manager.
|
||||
|
||||
>>> from sympy.utilities.exceptions import ignore_warnings
|
||||
>>> from sympy.physics.mechanics import Body, ReferenceFrame
|
||||
>>> with ignore_warnings(DeprecationWarning):
|
||||
... A = Body('A')
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> with ignore_warnings(DeprecationWarning):
|
||||
... B = Body('B', frame=N)
|
||||
>>> A.frame.set_ang_vel(N, 5*N.x)
|
||||
>>> A.ang_vel_in(B)
|
||||
5*N.x
|
||||
>>> A.ang_vel_in(N)
|
||||
5*N.x
|
||||
|
||||
"""
|
||||
|
||||
if isinstance(body, ReferenceFrame):
|
||||
frame=body
|
||||
elif isinstance(body, Body):
|
||||
frame = body.frame
|
||||
return self.frame.ang_vel_in(frame)
|
||||
|
||||
def dcm(self, body):
|
||||
"""
|
||||
Returns the direction cosine matrix of this body relative to the
|
||||
provided rigid body or reference frame.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
body: Body or ReferenceFrame
|
||||
The rigid body or reference frame to calculate the dcm.
|
||||
|
||||
Example
|
||||
=======
|
||||
|
||||
As Body has been deprecated, the following examples are for illustrative
|
||||
purposes only. The functionality of Body is fully captured by
|
||||
:class:`~.RigidBody` and :class:`~.Particle`. To ignore the deprecation
|
||||
warning we can use the ignore_warnings context manager.
|
||||
|
||||
>>> from sympy.utilities.exceptions import ignore_warnings
|
||||
>>> from sympy.physics.mechanics import Body
|
||||
>>> with ignore_warnings(DeprecationWarning):
|
||||
... A = Body('A')
|
||||
... B = Body('B')
|
||||
>>> A.frame.orient_axis(B.frame, B.frame.x, 5)
|
||||
>>> A.dcm(B)
|
||||
Matrix([
|
||||
[1, 0, 0],
|
||||
[0, cos(5), sin(5)],
|
||||
[0, -sin(5), cos(5)]])
|
||||
>>> A.dcm(B.frame)
|
||||
Matrix([
|
||||
[1, 0, 0],
|
||||
[0, cos(5), sin(5)],
|
||||
[0, -sin(5), cos(5)]])
|
||||
|
||||
"""
|
||||
|
||||
if isinstance(body, ReferenceFrame):
|
||||
frame=body
|
||||
elif isinstance(body, Body):
|
||||
frame = body.frame
|
||||
return self.frame.dcm(frame)
|
||||
|
||||
def parallel_axis(self, point, frame=None):
|
||||
"""Returns the inertia dyadic of the body with respect to another
|
||||
point.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
point : sympy.physics.vector.Point
|
||||
The point to express the inertia dyadic about.
|
||||
frame : sympy.physics.vector.ReferenceFrame
|
||||
The reference frame used to construct the dyadic.
|
||||
|
||||
Returns
|
||||
=======
|
||||
|
||||
inertia : sympy.physics.vector.Dyadic
|
||||
The inertia dyadic of the rigid body expressed about the provided
|
||||
point.
|
||||
|
||||
Example
|
||||
=======
|
||||
|
||||
As Body has been deprecated, the following examples are for illustrative
|
||||
purposes only. The functionality of Body is fully captured by
|
||||
:class:`~.RigidBody` and :class:`~.Particle`. To ignore the deprecation
|
||||
warning we can use the ignore_warnings context manager.
|
||||
|
||||
>>> from sympy.utilities.exceptions import ignore_warnings
|
||||
>>> from sympy.physics.mechanics import Body
|
||||
>>> with ignore_warnings(DeprecationWarning):
|
||||
... A = Body('A')
|
||||
>>> P = A.masscenter.locatenew('point', 3 * A.x + 5 * A.y)
|
||||
>>> A.parallel_axis(P).to_matrix(A.frame)
|
||||
Matrix([
|
||||
[A_ixx + 25*A_mass, A_ixy - 15*A_mass, A_izx],
|
||||
[A_ixy - 15*A_mass, A_iyy + 9*A_mass, A_iyz],
|
||||
[ A_izx, A_iyz, A_izz + 34*A_mass]])
|
||||
|
||||
"""
|
||||
if self.is_rigidbody:
|
||||
return RigidBody.parallel_axis(self, point, frame)
|
||||
return Particle.parallel_axis(self, point, frame)
|
||||
+94
@@ -0,0 +1,94 @@
|
||||
from abc import ABC, abstractmethod
|
||||
from sympy import Symbol, sympify
|
||||
from sympy.physics.vector import Point
|
||||
|
||||
__all__ = ['BodyBase']
|
||||
|
||||
|
||||
class BodyBase(ABC):
|
||||
"""Abstract class for body type objects."""
|
||||
def __init__(self, name, masscenter=None, mass=None):
|
||||
# Note: If frame=None, no auto-generated frame is created, because a
|
||||
# Particle does not need to have a frame by default.
|
||||
if not isinstance(name, str):
|
||||
raise TypeError('Supply a valid name.')
|
||||
self._name = name
|
||||
if mass is None:
|
||||
mass = Symbol(f'{name}_mass')
|
||||
if masscenter is None:
|
||||
masscenter = Point(f'{name}_masscenter')
|
||||
self.mass = mass
|
||||
self.masscenter = masscenter
|
||||
self.potential_energy = 0
|
||||
self.points = []
|
||||
|
||||
def __str__(self):
|
||||
return self.name
|
||||
|
||||
def __repr__(self):
|
||||
return (f'{self.__class__.__name__}({repr(self.name)}, masscenter='
|
||||
f'{repr(self.masscenter)}, mass={repr(self.mass)})')
|
||||
|
||||
@property
|
||||
def name(self):
|
||||
"""The name of the body."""
|
||||
return self._name
|
||||
|
||||
@property
|
||||
def masscenter(self):
|
||||
"""The body's center of mass."""
|
||||
return self._masscenter
|
||||
|
||||
@masscenter.setter
|
||||
def masscenter(self, point):
|
||||
if not isinstance(point, Point):
|
||||
raise TypeError("The body's center of mass must be a Point object.")
|
||||
self._masscenter = point
|
||||
|
||||
@property
|
||||
def mass(self):
|
||||
"""The body's mass."""
|
||||
return self._mass
|
||||
|
||||
@mass.setter
|
||||
def mass(self, mass):
|
||||
self._mass = sympify(mass)
|
||||
|
||||
@property
|
||||
def potential_energy(self):
|
||||
"""The potential energy of the body.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
>>> from sympy.physics.mechanics import Particle, Point
|
||||
>>> from sympy import symbols
|
||||
>>> m, g, h = symbols('m g h')
|
||||
>>> O = Point('O')
|
||||
>>> P = Particle('P', O, m)
|
||||
>>> P.potential_energy = m * g * h
|
||||
>>> P.potential_energy
|
||||
g*h*m
|
||||
|
||||
"""
|
||||
return self._potential_energy
|
||||
|
||||
@potential_energy.setter
|
||||
def potential_energy(self, scalar):
|
||||
self._potential_energy = sympify(scalar)
|
||||
|
||||
@abstractmethod
|
||||
def kinetic_energy(self, frame):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def linear_momentum(self, frame):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def angular_momentum(self, point, frame):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def parallel_axis(self, point, frame):
|
||||
pass
|
||||
+735
@@ -0,0 +1,735 @@
|
||||
from sympy.utilities import dict_merge
|
||||
from sympy.utilities.iterables import iterable
|
||||
from sympy.physics.vector import (Dyadic, Vector, ReferenceFrame,
|
||||
Point, dynamicsymbols)
|
||||
from sympy.physics.vector.printing import (vprint, vsprint, vpprint, vlatex,
|
||||
init_vprinting)
|
||||
from sympy.physics.mechanics.particle import Particle
|
||||
from sympy.physics.mechanics.rigidbody import RigidBody
|
||||
from sympy.simplify.simplify import simplify
|
||||
from sympy import Matrix, Mul, Derivative, sin, cos, tan, S
|
||||
from sympy.core.function import AppliedUndef
|
||||
from sympy.physics.mechanics.inertia import (inertia as _inertia,
|
||||
inertia_of_point_mass as _inertia_of_point_mass)
|
||||
from sympy.utilities.exceptions import sympy_deprecation_warning
|
||||
|
||||
__all__ = ['linear_momentum',
|
||||
'angular_momentum',
|
||||
'kinetic_energy',
|
||||
'potential_energy',
|
||||
'Lagrangian',
|
||||
'mechanics_printing',
|
||||
'mprint',
|
||||
'msprint',
|
||||
'mpprint',
|
||||
'mlatex',
|
||||
'msubs',
|
||||
'find_dynamicsymbols']
|
||||
|
||||
# These are functions that we've moved and renamed during extracting the
|
||||
# basic vector calculus code from the mechanics packages.
|
||||
|
||||
mprint = vprint
|
||||
msprint = vsprint
|
||||
mpprint = vpprint
|
||||
mlatex = vlatex
|
||||
|
||||
|
||||
def mechanics_printing(**kwargs):
|
||||
"""
|
||||
Initializes time derivative printing for all SymPy objects in
|
||||
mechanics module.
|
||||
"""
|
||||
|
||||
init_vprinting(**kwargs)
|
||||
|
||||
mechanics_printing.__doc__ = init_vprinting.__doc__
|
||||
|
||||
|
||||
def inertia(frame, ixx, iyy, izz, ixy=0, iyz=0, izx=0):
|
||||
sympy_deprecation_warning(
|
||||
"""
|
||||
The inertia function has been moved.
|
||||
Import it from "sympy.physics.mechanics".
|
||||
""",
|
||||
deprecated_since_version="1.13",
|
||||
active_deprecations_target="moved-mechanics-functions"
|
||||
)
|
||||
return _inertia(frame, ixx, iyy, izz, ixy, iyz, izx)
|
||||
|
||||
|
||||
def inertia_of_point_mass(mass, pos_vec, frame):
|
||||
sympy_deprecation_warning(
|
||||
"""
|
||||
The inertia_of_point_mass function has been moved.
|
||||
Import it from "sympy.physics.mechanics".
|
||||
""",
|
||||
deprecated_since_version="1.13",
|
||||
active_deprecations_target="moved-mechanics-functions"
|
||||
)
|
||||
return _inertia_of_point_mass(mass, pos_vec, frame)
|
||||
|
||||
|
||||
def linear_momentum(frame, *body):
|
||||
"""Linear momentum of the system.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
This function returns the linear momentum of a system of Particle's and/or
|
||||
RigidBody's. The linear momentum of a system is equal to the vector sum of
|
||||
the linear momentum of its constituents. Consider a system, S, comprised of
|
||||
a rigid body, A, and a particle, P. The linear momentum of the system, L,
|
||||
is equal to the vector sum of the linear momentum of the particle, L1, and
|
||||
the linear momentum of the rigid body, L2, i.e.
|
||||
|
||||
L = L1 + L2
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
frame : ReferenceFrame
|
||||
The frame in which linear momentum is desired.
|
||||
body1, body2, body3... : Particle and/or RigidBody
|
||||
The body (or bodies) whose linear momentum is required.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
>>> from sympy.physics.mechanics import Point, Particle, ReferenceFrame
|
||||
>>> from sympy.physics.mechanics import RigidBody, outer, linear_momentum
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> P = Point('P')
|
||||
>>> P.set_vel(N, 10 * N.x)
|
||||
>>> Pa = Particle('Pa', P, 1)
|
||||
>>> Ac = Point('Ac')
|
||||
>>> Ac.set_vel(N, 25 * N.y)
|
||||
>>> I = outer(N.x, N.x)
|
||||
>>> A = RigidBody('A', Ac, N, 20, (I, Ac))
|
||||
>>> linear_momentum(N, A, Pa)
|
||||
10*N.x + 500*N.y
|
||||
|
||||
"""
|
||||
|
||||
if not isinstance(frame, ReferenceFrame):
|
||||
raise TypeError('Please specify a valid ReferenceFrame')
|
||||
else:
|
||||
linear_momentum_sys = Vector(0)
|
||||
for e in body:
|
||||
if isinstance(e, (RigidBody, Particle)):
|
||||
linear_momentum_sys += e.linear_momentum(frame)
|
||||
else:
|
||||
raise TypeError('*body must have only Particle or RigidBody')
|
||||
return linear_momentum_sys
|
||||
|
||||
|
||||
def angular_momentum(point, frame, *body):
|
||||
"""Angular momentum of a system.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
This function returns the angular momentum of a system of Particle's and/or
|
||||
RigidBody's. The angular momentum of such a system is equal to the vector
|
||||
sum of the angular momentum of its constituents. Consider a system, S,
|
||||
comprised of a rigid body, A, and a particle, P. The angular momentum of
|
||||
the system, H, is equal to the vector sum of the angular momentum of the
|
||||
particle, H1, and the angular momentum of the rigid body, H2, i.e.
|
||||
|
||||
H = H1 + H2
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
point : Point
|
||||
The point about which angular momentum of the system is desired.
|
||||
frame : ReferenceFrame
|
||||
The frame in which angular momentum is desired.
|
||||
body1, body2, body3... : Particle and/or RigidBody
|
||||
The body (or bodies) whose angular momentum is required.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
>>> from sympy.physics.mechanics import Point, Particle, ReferenceFrame
|
||||
>>> from sympy.physics.mechanics import RigidBody, outer, angular_momentum
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> O = Point('O')
|
||||
>>> O.set_vel(N, 0 * N.x)
|
||||
>>> P = O.locatenew('P', 1 * N.x)
|
||||
>>> P.set_vel(N, 10 * N.x)
|
||||
>>> Pa = Particle('Pa', P, 1)
|
||||
>>> Ac = O.locatenew('Ac', 2 * N.y)
|
||||
>>> Ac.set_vel(N, 5 * N.y)
|
||||
>>> a = ReferenceFrame('a')
|
||||
>>> a.set_ang_vel(N, 10 * N.z)
|
||||
>>> I = outer(N.z, N.z)
|
||||
>>> A = RigidBody('A', Ac, a, 20, (I, Ac))
|
||||
>>> angular_momentum(O, N, Pa, A)
|
||||
10*N.z
|
||||
|
||||
"""
|
||||
|
||||
if not isinstance(frame, ReferenceFrame):
|
||||
raise TypeError('Please enter a valid ReferenceFrame')
|
||||
if not isinstance(point, Point):
|
||||
raise TypeError('Please specify a valid Point')
|
||||
else:
|
||||
angular_momentum_sys = Vector(0)
|
||||
for e in body:
|
||||
if isinstance(e, (RigidBody, Particle)):
|
||||
angular_momentum_sys += e.angular_momentum(point, frame)
|
||||
else:
|
||||
raise TypeError('*body must have only Particle or RigidBody')
|
||||
return angular_momentum_sys
|
||||
|
||||
|
||||
def kinetic_energy(frame, *body):
|
||||
"""Kinetic energy of a multibody system.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
This function returns the kinetic energy of a system of Particle's and/or
|
||||
RigidBody's. The kinetic energy of such a system is equal to the sum of
|
||||
the kinetic energies of its constituents. Consider a system, S, comprising
|
||||
a rigid body, A, and a particle, P. The kinetic energy of the system, T,
|
||||
is equal to the vector sum of the kinetic energy of the particle, T1, and
|
||||
the kinetic energy of the rigid body, T2, i.e.
|
||||
|
||||
T = T1 + T2
|
||||
|
||||
Kinetic energy is a scalar.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
frame : ReferenceFrame
|
||||
The frame in which the velocity or angular velocity of the body is
|
||||
defined.
|
||||
body1, body2, body3... : Particle and/or RigidBody
|
||||
The body (or bodies) whose kinetic energy is required.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
>>> from sympy.physics.mechanics import Point, Particle, ReferenceFrame
|
||||
>>> from sympy.physics.mechanics import RigidBody, outer, kinetic_energy
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> O = Point('O')
|
||||
>>> O.set_vel(N, 0 * N.x)
|
||||
>>> P = O.locatenew('P', 1 * N.x)
|
||||
>>> P.set_vel(N, 10 * N.x)
|
||||
>>> Pa = Particle('Pa', P, 1)
|
||||
>>> Ac = O.locatenew('Ac', 2 * N.y)
|
||||
>>> Ac.set_vel(N, 5 * N.y)
|
||||
>>> a = ReferenceFrame('a')
|
||||
>>> a.set_ang_vel(N, 10 * N.z)
|
||||
>>> I = outer(N.z, N.z)
|
||||
>>> A = RigidBody('A', Ac, a, 20, (I, Ac))
|
||||
>>> kinetic_energy(N, Pa, A)
|
||||
350
|
||||
|
||||
"""
|
||||
|
||||
if not isinstance(frame, ReferenceFrame):
|
||||
raise TypeError('Please enter a valid ReferenceFrame')
|
||||
ke_sys = S.Zero
|
||||
for e in body:
|
||||
if isinstance(e, (RigidBody, Particle)):
|
||||
ke_sys += e.kinetic_energy(frame)
|
||||
else:
|
||||
raise TypeError('*body must have only Particle or RigidBody')
|
||||
return ke_sys
|
||||
|
||||
|
||||
def potential_energy(*body):
|
||||
"""Potential energy of a multibody system.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
This function returns the potential energy of a system of Particle's and/or
|
||||
RigidBody's. The potential energy of such a system is equal to the sum of
|
||||
the potential energy of its constituents. Consider a system, S, comprising
|
||||
a rigid body, A, and a particle, P. The potential energy of the system, V,
|
||||
is equal to the vector sum of the potential energy of the particle, V1, and
|
||||
the potential energy of the rigid body, V2, i.e.
|
||||
|
||||
V = V1 + V2
|
||||
|
||||
Potential energy is a scalar.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
body1, body2, body3... : Particle and/or RigidBody
|
||||
The body (or bodies) whose potential energy is required.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
>>> from sympy.physics.mechanics import Point, Particle, ReferenceFrame
|
||||
>>> from sympy.physics.mechanics import RigidBody, outer, potential_energy
|
||||
>>> from sympy import symbols
|
||||
>>> M, m, g, h = symbols('M m g h')
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> O = Point('O')
|
||||
>>> O.set_vel(N, 0 * N.x)
|
||||
>>> P = O.locatenew('P', 1 * N.x)
|
||||
>>> Pa = Particle('Pa', P, m)
|
||||
>>> Ac = O.locatenew('Ac', 2 * N.y)
|
||||
>>> a = ReferenceFrame('a')
|
||||
>>> I = outer(N.z, N.z)
|
||||
>>> A = RigidBody('A', Ac, a, M, (I, Ac))
|
||||
>>> Pa.potential_energy = m * g * h
|
||||
>>> A.potential_energy = M * g * h
|
||||
>>> potential_energy(Pa, A)
|
||||
M*g*h + g*h*m
|
||||
|
||||
"""
|
||||
|
||||
pe_sys = S.Zero
|
||||
for e in body:
|
||||
if isinstance(e, (RigidBody, Particle)):
|
||||
pe_sys += e.potential_energy
|
||||
else:
|
||||
raise TypeError('*body must have only Particle or RigidBody')
|
||||
return pe_sys
|
||||
|
||||
|
||||
def gravity(acceleration, *bodies):
|
||||
from sympy.physics.mechanics.loads import gravity as _gravity
|
||||
sympy_deprecation_warning(
|
||||
"""
|
||||
The gravity function has been moved.
|
||||
Import it from "sympy.physics.mechanics.loads".
|
||||
""",
|
||||
deprecated_since_version="1.13",
|
||||
active_deprecations_target="moved-mechanics-functions"
|
||||
)
|
||||
return _gravity(acceleration, *bodies)
|
||||
|
||||
|
||||
def center_of_mass(point, *bodies):
|
||||
"""
|
||||
Returns the position vector from the given point to the center of mass
|
||||
of the given bodies(particles or rigidbodies).
|
||||
|
||||
Example
|
||||
=======
|
||||
|
||||
>>> from sympy import symbols, S
|
||||
>>> from sympy.physics.vector import Point
|
||||
>>> from sympy.physics.mechanics import Particle, ReferenceFrame, RigidBody, outer
|
||||
>>> from sympy.physics.mechanics.functions import center_of_mass
|
||||
>>> a = ReferenceFrame('a')
|
||||
>>> m = symbols('m', real=True)
|
||||
>>> p1 = Particle('p1', Point('p1_pt'), S(1))
|
||||
>>> p2 = Particle('p2', Point('p2_pt'), S(2))
|
||||
>>> p3 = Particle('p3', Point('p3_pt'), S(3))
|
||||
>>> p4 = Particle('p4', Point('p4_pt'), m)
|
||||
>>> b_f = ReferenceFrame('b_f')
|
||||
>>> b_cm = Point('b_cm')
|
||||
>>> mb = symbols('mb')
|
||||
>>> b = RigidBody('b', b_cm, b_f, mb, (outer(b_f.x, b_f.x), b_cm))
|
||||
>>> p2.point.set_pos(p1.point, a.x)
|
||||
>>> p3.point.set_pos(p1.point, a.x + a.y)
|
||||
>>> p4.point.set_pos(p1.point, a.y)
|
||||
>>> b.masscenter.set_pos(p1.point, a.y + a.z)
|
||||
>>> point_o=Point('o')
|
||||
>>> point_o.set_pos(p1.point, center_of_mass(p1.point, p1, p2, p3, p4, b))
|
||||
>>> expr = 5/(m + mb + 6)*a.x + (m + mb + 3)/(m + mb + 6)*a.y + mb/(m + mb + 6)*a.z
|
||||
>>> point_o.pos_from(p1.point)
|
||||
5/(m + mb + 6)*a.x + (m + mb + 3)/(m + mb + 6)*a.y + mb/(m + mb + 6)*a.z
|
||||
|
||||
"""
|
||||
if not bodies:
|
||||
raise TypeError("No bodies(instances of Particle or Rigidbody) were passed.")
|
||||
|
||||
total_mass = 0
|
||||
vec = Vector(0)
|
||||
for i in bodies:
|
||||
total_mass += i.mass
|
||||
|
||||
masscenter = getattr(i, 'masscenter', None)
|
||||
if masscenter is None:
|
||||
masscenter = i.point
|
||||
vec += i.mass*masscenter.pos_from(point)
|
||||
|
||||
return vec/total_mass
|
||||
|
||||
|
||||
def Lagrangian(frame, *body):
|
||||
"""Lagrangian of a multibody system.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
This function returns the Lagrangian of a system of Particle's and/or
|
||||
RigidBody's. The Lagrangian of such a system is equal to the difference
|
||||
between the kinetic energies and potential energies of its constituents. If
|
||||
T and V are the kinetic and potential energies of a system then it's
|
||||
Lagrangian, L, is defined as
|
||||
|
||||
L = T - V
|
||||
|
||||
The Lagrangian is a scalar.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
frame : ReferenceFrame
|
||||
The frame in which the velocity or angular velocity of the body is
|
||||
defined to determine the kinetic energy.
|
||||
|
||||
body1, body2, body3... : Particle and/or RigidBody
|
||||
The body (or bodies) whose Lagrangian is required.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
>>> from sympy.physics.mechanics import Point, Particle, ReferenceFrame
|
||||
>>> from sympy.physics.mechanics import RigidBody, outer, Lagrangian
|
||||
>>> from sympy import symbols
|
||||
>>> M, m, g, h = symbols('M m g h')
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> O = Point('O')
|
||||
>>> O.set_vel(N, 0 * N.x)
|
||||
>>> P = O.locatenew('P', 1 * N.x)
|
||||
>>> P.set_vel(N, 10 * N.x)
|
||||
>>> Pa = Particle('Pa', P, 1)
|
||||
>>> Ac = O.locatenew('Ac', 2 * N.y)
|
||||
>>> Ac.set_vel(N, 5 * N.y)
|
||||
>>> a = ReferenceFrame('a')
|
||||
>>> a.set_ang_vel(N, 10 * N.z)
|
||||
>>> I = outer(N.z, N.z)
|
||||
>>> A = RigidBody('A', Ac, a, 20, (I, Ac))
|
||||
>>> Pa.potential_energy = m * g * h
|
||||
>>> A.potential_energy = M * g * h
|
||||
>>> Lagrangian(N, Pa, A)
|
||||
-M*g*h - g*h*m + 350
|
||||
|
||||
"""
|
||||
|
||||
if not isinstance(frame, ReferenceFrame):
|
||||
raise TypeError('Please supply a valid ReferenceFrame')
|
||||
for e in body:
|
||||
if not isinstance(e, (RigidBody, Particle)):
|
||||
raise TypeError('*body must have only Particle or RigidBody')
|
||||
return kinetic_energy(frame, *body) - potential_energy(*body)
|
||||
|
||||
|
||||
def find_dynamicsymbols(expression, exclude=None, reference_frame=None):
|
||||
"""Find all dynamicsymbols in expression.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
If the optional ``exclude`` kwarg is used, only dynamicsymbols
|
||||
not in the iterable ``exclude`` are returned.
|
||||
If we intend to apply this function on a vector, the optional
|
||||
``reference_frame`` is also used to inform about the corresponding frame
|
||||
with respect to which the dynamic symbols of the given vector is to be
|
||||
determined.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
expression : SymPy expression
|
||||
|
||||
exclude : iterable of dynamicsymbols, optional
|
||||
|
||||
reference_frame : ReferenceFrame, optional
|
||||
The frame with respect to which the dynamic symbols of the
|
||||
given vector is to be determined.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
>>> from sympy.physics.mechanics import dynamicsymbols, find_dynamicsymbols
|
||||
>>> from sympy.physics.mechanics import ReferenceFrame
|
||||
>>> x, y = dynamicsymbols('x, y')
|
||||
>>> expr = x + x.diff()*y
|
||||
>>> find_dynamicsymbols(expr)
|
||||
{x(t), y(t), Derivative(x(t), t)}
|
||||
>>> find_dynamicsymbols(expr, exclude=[x, y])
|
||||
{Derivative(x(t), t)}
|
||||
>>> a, b, c = dynamicsymbols('a, b, c')
|
||||
>>> A = ReferenceFrame('A')
|
||||
>>> v = a * A.x + b * A.y + c * A.z
|
||||
>>> find_dynamicsymbols(v, reference_frame=A)
|
||||
{a(t), b(t), c(t)}
|
||||
|
||||
"""
|
||||
t_set = {dynamicsymbols._t}
|
||||
if exclude:
|
||||
if iterable(exclude):
|
||||
exclude_set = set(exclude)
|
||||
else:
|
||||
raise TypeError("exclude kwarg must be iterable")
|
||||
else:
|
||||
exclude_set = set()
|
||||
if isinstance(expression, Vector):
|
||||
if reference_frame is None:
|
||||
raise ValueError("You must provide reference_frame when passing a "
|
||||
"vector expression, got %s." % reference_frame)
|
||||
else:
|
||||
expression = expression.to_matrix(reference_frame)
|
||||
return {i for i in expression.atoms(AppliedUndef, Derivative) if
|
||||
i.free_symbols == t_set} - exclude_set
|
||||
|
||||
|
||||
def msubs(expr, *sub_dicts, smart=False, **kwargs):
|
||||
"""A custom subs for use on expressions derived in physics.mechanics.
|
||||
|
||||
Traverses the expression tree once, performing the subs found in sub_dicts.
|
||||
Terms inside ``Derivative`` expressions are ignored:
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
>>> from sympy.physics.mechanics import dynamicsymbols, msubs
|
||||
>>> x = dynamicsymbols('x')
|
||||
>>> msubs(x.diff() + x, {x: 1})
|
||||
Derivative(x(t), t) + 1
|
||||
|
||||
Note that sub_dicts can be a single dictionary, or several dictionaries:
|
||||
|
||||
>>> x, y, z = dynamicsymbols('x, y, z')
|
||||
>>> sub1 = {x: 1, y: 2}
|
||||
>>> sub2 = {z: 3, x.diff(): 4}
|
||||
>>> msubs(x.diff() + x + y + z, sub1, sub2)
|
||||
10
|
||||
|
||||
If smart=True (default False), also checks for conditions that may result
|
||||
in ``nan``, but if simplified would yield a valid expression. For example:
|
||||
|
||||
>>> from sympy import sin, tan
|
||||
>>> (sin(x)/tan(x)).subs(x, 0)
|
||||
nan
|
||||
>>> msubs(sin(x)/tan(x), {x: 0}, smart=True)
|
||||
1
|
||||
|
||||
It does this by first replacing all ``tan`` with ``sin/cos``. Then each
|
||||
node is traversed. If the node is a fraction, subs is first evaluated on
|
||||
the denominator. If this results in 0, simplification of the entire
|
||||
fraction is attempted. Using this selective simplification, only
|
||||
subexpressions that result in 1/0 are targeted, resulting in faster
|
||||
performance.
|
||||
|
||||
"""
|
||||
|
||||
sub_dict = dict_merge(*sub_dicts)
|
||||
if smart:
|
||||
func = _smart_subs
|
||||
elif hasattr(expr, 'msubs'):
|
||||
return expr.msubs(sub_dict)
|
||||
else:
|
||||
func = lambda expr, sub_dict: _crawl(expr, _sub_func, sub_dict)
|
||||
if isinstance(expr, (Matrix, Vector, Dyadic)):
|
||||
return expr.applyfunc(lambda x: func(x, sub_dict))
|
||||
else:
|
||||
return func(expr, sub_dict)
|
||||
|
||||
|
||||
def _crawl(expr, func, *args, **kwargs):
|
||||
"""Crawl the expression tree, and apply func to every node."""
|
||||
val = func(expr, *args, **kwargs)
|
||||
if val is not None:
|
||||
return val
|
||||
new_args = (_crawl(arg, func, *args, **kwargs) for arg in expr.args)
|
||||
return expr.func(*new_args)
|
||||
|
||||
|
||||
def _sub_func(expr, sub_dict):
|
||||
"""Perform direct matching substitution, ignoring derivatives."""
|
||||
if expr in sub_dict:
|
||||
return sub_dict[expr]
|
||||
elif not expr.args or expr.is_Derivative:
|
||||
return expr
|
||||
|
||||
|
||||
def _tan_repl_func(expr):
|
||||
"""Replace tan with sin/cos."""
|
||||
if isinstance(expr, tan):
|
||||
return sin(*expr.args) / cos(*expr.args)
|
||||
elif not expr.args or expr.is_Derivative:
|
||||
return expr
|
||||
|
||||
|
||||
def _smart_subs(expr, sub_dict):
|
||||
"""Performs subs, checking for conditions that may result in `nan` or
|
||||
`oo`, and attempts to simplify them out.
|
||||
|
||||
The expression tree is traversed twice, and the following steps are
|
||||
performed on each expression node:
|
||||
- First traverse:
|
||||
Replace all `tan` with `sin/cos`.
|
||||
- Second traverse:
|
||||
If node is a fraction, check if the denominator evaluates to 0.
|
||||
If so, attempt to simplify it out. Then if node is in sub_dict,
|
||||
sub in the corresponding value.
|
||||
|
||||
"""
|
||||
expr = _crawl(expr, _tan_repl_func)
|
||||
|
||||
def _recurser(expr, sub_dict):
|
||||
# Decompose the expression into num, den
|
||||
num, den = _fraction_decomp(expr)
|
||||
if den != 1:
|
||||
# If there is a non trivial denominator, we need to handle it
|
||||
denom_subbed = _recurser(den, sub_dict)
|
||||
if denom_subbed.evalf() == 0:
|
||||
# If denom is 0 after this, attempt to simplify the bad expr
|
||||
expr = simplify(expr)
|
||||
else:
|
||||
# Expression won't result in nan, find numerator
|
||||
num_subbed = _recurser(num, sub_dict)
|
||||
return num_subbed / denom_subbed
|
||||
# We have to crawl the tree manually, because `expr` may have been
|
||||
# modified in the simplify step. First, perform subs as normal:
|
||||
val = _sub_func(expr, sub_dict)
|
||||
if val is not None:
|
||||
return val
|
||||
new_args = (_recurser(arg, sub_dict) for arg in expr.args)
|
||||
return expr.func(*new_args)
|
||||
return _recurser(expr, sub_dict)
|
||||
|
||||
|
||||
def _fraction_decomp(expr):
|
||||
"""Return num, den such that expr = num/den."""
|
||||
if not isinstance(expr, Mul):
|
||||
return expr, 1
|
||||
num = []
|
||||
den = []
|
||||
for a in expr.args:
|
||||
if a.is_Pow and a.args[1] < 0:
|
||||
den.append(1 / a)
|
||||
else:
|
||||
num.append(a)
|
||||
if not den:
|
||||
return expr, 1
|
||||
num = Mul(*num)
|
||||
den = Mul(*den)
|
||||
return num, den
|
||||
|
||||
|
||||
def _f_list_parser(fl, ref_frame):
|
||||
"""Parses the provided forcelist composed of items
|
||||
of the form (obj, force).
|
||||
Returns a tuple containing:
|
||||
vel_list: The velocity (ang_vel for Frames, vel for Points) in
|
||||
the provided reference frame.
|
||||
f_list: The forces.
|
||||
|
||||
Used internally in the KanesMethod and LagrangesMethod classes.
|
||||
|
||||
"""
|
||||
def flist_iter():
|
||||
for pair in fl:
|
||||
obj, force = pair
|
||||
if isinstance(obj, ReferenceFrame):
|
||||
yield obj.ang_vel_in(ref_frame), force
|
||||
elif isinstance(obj, Point):
|
||||
yield obj.vel(ref_frame), force
|
||||
else:
|
||||
raise TypeError('First entry in each forcelist pair must '
|
||||
'be a point or frame.')
|
||||
|
||||
if not fl:
|
||||
vel_list, f_list = (), ()
|
||||
else:
|
||||
unzip = lambda l: list(zip(*l)) if l[0] else [(), ()]
|
||||
vel_list, f_list = unzip(list(flist_iter()))
|
||||
return vel_list, f_list
|
||||
|
||||
|
||||
def _validate_coordinates(coordinates=None, speeds=None, check_duplicates=True,
|
||||
is_dynamicsymbols=True, u_auxiliary=None):
|
||||
"""Validate the generalized coordinates and generalized speeds.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
coordinates : iterable, optional
|
||||
Generalized coordinates to be validated.
|
||||
speeds : iterable, optional
|
||||
Generalized speeds to be validated.
|
||||
check_duplicates : bool, optional
|
||||
Checks if there are duplicates in the generalized coordinates and
|
||||
generalized speeds. If so it will raise a ValueError. The default is
|
||||
True.
|
||||
is_dynamicsymbols : iterable, optional
|
||||
Checks if all the generalized coordinates and generalized speeds are
|
||||
dynamicsymbols. If any is not a dynamicsymbol, a ValueError will be
|
||||
raised. The default is True.
|
||||
u_auxiliary : iterable, optional
|
||||
Auxiliary generalized speeds to be validated.
|
||||
|
||||
"""
|
||||
t_set = {dynamicsymbols._t}
|
||||
# Convert input to iterables
|
||||
if coordinates is None:
|
||||
coordinates = []
|
||||
elif not iterable(coordinates):
|
||||
coordinates = [coordinates]
|
||||
if speeds is None:
|
||||
speeds = []
|
||||
elif not iterable(speeds):
|
||||
speeds = [speeds]
|
||||
if u_auxiliary is None:
|
||||
u_auxiliary = []
|
||||
elif not iterable(u_auxiliary):
|
||||
u_auxiliary = [u_auxiliary]
|
||||
|
||||
msgs = []
|
||||
if check_duplicates: # Check for duplicates
|
||||
seen = set()
|
||||
coord_duplicates = {x for x in coordinates if x in seen or seen.add(x)}
|
||||
seen = set()
|
||||
speed_duplicates = {x for x in speeds if x in seen or seen.add(x)}
|
||||
seen = set()
|
||||
aux_duplicates = {x for x in u_auxiliary if x in seen or seen.add(x)}
|
||||
overlap_coords = set(coordinates).intersection(speeds)
|
||||
overlap_aux = set(coordinates).union(speeds).intersection(u_auxiliary)
|
||||
if coord_duplicates:
|
||||
msgs.append(f'The generalized coordinates {coord_duplicates} are '
|
||||
f'duplicated, all generalized coordinates should be '
|
||||
f'unique.')
|
||||
if speed_duplicates:
|
||||
msgs.append(f'The generalized speeds {speed_duplicates} are '
|
||||
f'duplicated, all generalized speeds should be unique.')
|
||||
if aux_duplicates:
|
||||
msgs.append(f'The auxiliary speeds {aux_duplicates} are duplicated,'
|
||||
f' all auxiliary speeds should be unique.')
|
||||
if overlap_coords:
|
||||
msgs.append(f'{overlap_coords} are defined as both generalized '
|
||||
f'coordinates and generalized speeds.')
|
||||
if overlap_aux:
|
||||
msgs.append(f'The auxiliary speeds {overlap_aux} are also defined '
|
||||
f'as generalized coordinates or generalized speeds.')
|
||||
if is_dynamicsymbols: # Check whether all coordinates are dynamicsymbols
|
||||
for coordinate in coordinates:
|
||||
if not (isinstance(coordinate, (AppliedUndef, Derivative)) and
|
||||
coordinate.free_symbols == t_set):
|
||||
msgs.append(f'Generalized coordinate "{coordinate}" is not a '
|
||||
f'dynamicsymbol.')
|
||||
for speed in speeds:
|
||||
if not (isinstance(speed, (AppliedUndef, Derivative)) and
|
||||
speed.free_symbols == t_set):
|
||||
msgs.append(
|
||||
f'Generalized speed "{speed}" is not a dynamicsymbol.')
|
||||
for aux in u_auxiliary:
|
||||
if not (isinstance(aux, (AppliedUndef, Derivative)) and
|
||||
aux.free_symbols == t_set):
|
||||
msgs.append(
|
||||
f'Auxiliary speed "{aux}" is not a dynamicsymbol.')
|
||||
if msgs:
|
||||
raise ValueError('\n'.join(msgs))
|
||||
|
||||
|
||||
def _parse_linear_solver(linear_solver):
|
||||
"""Helper function to retrieve a specified linear solver."""
|
||||
if callable(linear_solver):
|
||||
return linear_solver
|
||||
return lambda A, b: Matrix.solve(A, b, method=linear_solver)
|
||||
+199
@@ -0,0 +1,199 @@
|
||||
from sympy import sympify
|
||||
from sympy.physics.vector import Point, Dyadic, ReferenceFrame, outer
|
||||
from collections import namedtuple
|
||||
|
||||
__all__ = ['inertia', 'inertia_of_point_mass', 'Inertia']
|
||||
|
||||
|
||||
def inertia(frame, ixx, iyy, izz, ixy=0, iyz=0, izx=0):
|
||||
"""Simple way to create inertia Dyadic object.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
Creates an inertia Dyadic based on the given tensor values and a body-fixed
|
||||
reference frame.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
frame : ReferenceFrame
|
||||
The frame the inertia is defined in.
|
||||
ixx : Sympifyable
|
||||
The xx element in the inertia dyadic.
|
||||
iyy : Sympifyable
|
||||
The yy element in the inertia dyadic.
|
||||
izz : Sympifyable
|
||||
The zz element in the inertia dyadic.
|
||||
ixy : Sympifyable
|
||||
The xy element in the inertia dyadic.
|
||||
iyz : Sympifyable
|
||||
The yz element in the inertia dyadic.
|
||||
izx : Sympifyable
|
||||
The zx element in the inertia dyadic.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
>>> from sympy.physics.mechanics import ReferenceFrame, inertia
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> inertia(N, 1, 2, 3)
|
||||
(N.x|N.x) + 2*(N.y|N.y) + 3*(N.z|N.z)
|
||||
|
||||
"""
|
||||
|
||||
if not isinstance(frame, ReferenceFrame):
|
||||
raise TypeError('Need to define the inertia in a frame')
|
||||
ixx, iyy, izz = sympify(ixx), sympify(iyy), sympify(izz)
|
||||
ixy, iyz, izx = sympify(ixy), sympify(iyz), sympify(izx)
|
||||
return (ixx*outer(frame.x, frame.x) + ixy*outer(frame.x, frame.y) +
|
||||
izx*outer(frame.x, frame.z) + ixy*outer(frame.y, frame.x) +
|
||||
iyy*outer(frame.y, frame.y) + iyz*outer(frame.y, frame.z) +
|
||||
izx*outer(frame.z, frame.x) + iyz*outer(frame.z, frame.y) +
|
||||
izz*outer(frame.z, frame.z))
|
||||
|
||||
|
||||
def inertia_of_point_mass(mass, pos_vec, frame):
|
||||
"""Inertia dyadic of a point mass relative to point O.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
mass : Sympifyable
|
||||
Mass of the point mass
|
||||
pos_vec : Vector
|
||||
Position from point O to point mass
|
||||
frame : ReferenceFrame
|
||||
Reference frame to express the dyadic in
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
>>> from sympy import symbols
|
||||
>>> from sympy.physics.mechanics import ReferenceFrame, inertia_of_point_mass
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> r, m = symbols('r m')
|
||||
>>> px = r * N.x
|
||||
>>> inertia_of_point_mass(m, px, N)
|
||||
m*r**2*(N.y|N.y) + m*r**2*(N.z|N.z)
|
||||
|
||||
"""
|
||||
|
||||
return mass*(
|
||||
(outer(frame.x, frame.x) +
|
||||
outer(frame.y, frame.y) +
|
||||
outer(frame.z, frame.z)) *
|
||||
(pos_vec.dot(pos_vec)) - outer(pos_vec, pos_vec))
|
||||
|
||||
|
||||
class Inertia(namedtuple('Inertia', ['dyadic', 'point'])):
|
||||
"""Inertia object consisting of a Dyadic and a Point of reference.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
This is a simple class to store the Point and Dyadic, belonging to an
|
||||
inertia.
|
||||
|
||||
Attributes
|
||||
==========
|
||||
|
||||
dyadic : Dyadic
|
||||
The dyadic of the inertia.
|
||||
point : Point
|
||||
The reference point of the inertia.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
>>> from sympy.physics.mechanics import ReferenceFrame, Point, Inertia
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> Po = Point('Po')
|
||||
>>> Inertia(N.x.outer(N.x) + N.y.outer(N.y) + N.z.outer(N.z), Po)
|
||||
((N.x|N.x) + (N.y|N.y) + (N.z|N.z), Po)
|
||||
|
||||
In the example above the Dyadic was created manually, one can however also
|
||||
use the ``inertia`` function for this or the class method ``from_tensor`` as
|
||||
shown below.
|
||||
|
||||
>>> Inertia.from_inertia_scalars(Po, N, 1, 1, 1)
|
||||
((N.x|N.x) + (N.y|N.y) + (N.z|N.z), Po)
|
||||
|
||||
"""
|
||||
__slots__ = ()
|
||||
|
||||
def __new__(cls, dyadic, point):
|
||||
# Switch order if given in the wrong order
|
||||
if isinstance(dyadic, Point) and isinstance(point, Dyadic):
|
||||
point, dyadic = dyadic, point
|
||||
if not isinstance(point, Point):
|
||||
raise TypeError('Reference point should be of type Point')
|
||||
if not isinstance(dyadic, Dyadic):
|
||||
raise TypeError('Inertia value should be expressed as a Dyadic')
|
||||
return super().__new__(cls, dyadic, point)
|
||||
|
||||
@classmethod
|
||||
def from_inertia_scalars(cls, point, frame, ixx, iyy, izz, ixy=0, iyz=0,
|
||||
izx=0):
|
||||
"""Simple way to create an Inertia object based on the tensor values.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
This class method uses the :func`~.inertia` to create the Dyadic based
|
||||
on the tensor values.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
point : Point
|
||||
The reference point of the inertia.
|
||||
frame : ReferenceFrame
|
||||
The frame the inertia is defined in.
|
||||
ixx : Sympifyable
|
||||
The xx element in the inertia dyadic.
|
||||
iyy : Sympifyable
|
||||
The yy element in the inertia dyadic.
|
||||
izz : Sympifyable
|
||||
The zz element in the inertia dyadic.
|
||||
ixy : Sympifyable
|
||||
The xy element in the inertia dyadic.
|
||||
iyz : Sympifyable
|
||||
The yz element in the inertia dyadic.
|
||||
izx : Sympifyable
|
||||
The zx element in the inertia dyadic.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
>>> from sympy import symbols
|
||||
>>> from sympy.physics.mechanics import ReferenceFrame, Point, Inertia
|
||||
>>> ixx, iyy, izz, ixy, iyz, izx = symbols('ixx iyy izz ixy iyz izx')
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> P = Point('P')
|
||||
>>> I = Inertia.from_inertia_scalars(P, N, ixx, iyy, izz, ixy, iyz, izx)
|
||||
|
||||
The tensor values can easily be seen when converting the dyadic to a
|
||||
matrix.
|
||||
|
||||
>>> I.dyadic.to_matrix(N)
|
||||
Matrix([
|
||||
[ixx, ixy, izx],
|
||||
[ixy, iyy, iyz],
|
||||
[izx, iyz, izz]])
|
||||
|
||||
"""
|
||||
return cls(inertia(frame, ixx, iyy, izz, ixy, iyz, izx), point)
|
||||
|
||||
def __add__(self, other):
|
||||
raise TypeError(f"unsupported operand type(s) for +: "
|
||||
f"'{self.__class__.__name__}' and "
|
||||
f"'{other.__class__.__name__}'")
|
||||
|
||||
def __mul__(self, other):
|
||||
raise TypeError(f"unsupported operand type(s) for *: "
|
||||
f"'{self.__class__.__name__}' and "
|
||||
f"'{other.__class__.__name__}'")
|
||||
|
||||
__radd__ = __add__
|
||||
__rmul__ = __mul__
|
||||
+2188
File diff suppressed because it is too large
Load Diff
+318
@@ -0,0 +1,318 @@
|
||||
from sympy.physics.mechanics import (Body, Lagrangian, KanesMethod, LagrangesMethod,
|
||||
RigidBody, Particle)
|
||||
from sympy.physics.mechanics.body_base import BodyBase
|
||||
from sympy.physics.mechanics.method import _Methods
|
||||
from sympy import Matrix
|
||||
from sympy.utilities.exceptions import sympy_deprecation_warning
|
||||
|
||||
__all__ = ['JointsMethod']
|
||||
|
||||
|
||||
class JointsMethod(_Methods):
|
||||
"""Method for formulating the equations of motion using a set of interconnected bodies with joints.
|
||||
|
||||
.. deprecated:: 1.13
|
||||
The JointsMethod class is deprecated. Its functionality has been
|
||||
replaced by the new :class:`~.System` class.
|
||||
|
||||
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
|
||||
========
|
||||
|
||||
As Body and JointsMethod have been deprecated, the following examples are
|
||||
for illustrative purposes only. The functionality of Body is fully captured
|
||||
by :class:`~.RigidBody` and :class:`~.Particle` and the functionality of
|
||||
JointsMethod is fully captured by :class:`~.System`. To ignore the
|
||||
deprecation warning we can use the ignore_warnings context manager.
|
||||
|
||||
>>> from sympy.utilities.exceptions import ignore_warnings
|
||||
|
||||
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')
|
||||
>>> with ignore_warnings(DeprecationWarning):
|
||||
... 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)
|
||||
>>> with ignore_warnings(DeprecationWarning):
|
||||
... 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):
|
||||
sympy_deprecation_warning(
|
||||
"""
|
||||
The JointsMethod class is deprecated.
|
||||
Its functionality has been replaced by the new System class.
|
||||
""",
|
||||
deprecated_since_version="1.13",
|
||||
active_deprecations_target="deprecated-mechanics-jointsmethod"
|
||||
)
|
||||
if isinstance(newtonion, BodyBase):
|
||||
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:
|
||||
if isinstance(body, Body):
|
||||
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 not isinstance(body, Body):
|
||||
bodylist.append(body)
|
||||
continue
|
||||
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
|
||||
========
|
||||
|
||||
As Body and JointsMethod have been deprecated, the following examples
|
||||
are for illustrative purposes only. The functionality of Body is fully
|
||||
captured by :class:`~.RigidBody` and :class:`~.Particle` and the
|
||||
functionality of JointsMethod is fully captured by :class:`~.System`. To
|
||||
ignore the deprecation warning we can use the ignore_warnings context
|
||||
manager.
|
||||
|
||||
>>> from sympy.utilities.exceptions import ignore_warnings
|
||||
|
||||
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')
|
||||
>>> with ignore_warnings(DeprecationWarning):
|
||||
... 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)
|
||||
>>> with ignore_warnings(DeprecationWarning):
|
||||
... 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.matrixbase.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)
|
||||
+859
@@ -0,0 +1,859 @@
|
||||
from sympy import zeros, Matrix, diff, eye, linear_eq_to_matrix
|
||||
from sympy.core.sorting import default_sort_key
|
||||
from sympy.physics.vector import (ReferenceFrame, dynamicsymbols,
|
||||
partial_velocity)
|
||||
from sympy.physics.mechanics.method import _Methods
|
||||
from sympy.physics.mechanics.particle import Particle
|
||||
from sympy.physics.mechanics.rigidbody import RigidBody
|
||||
from sympy.physics.mechanics.functions import (msubs, find_dynamicsymbols,
|
||||
_f_list_parser,
|
||||
_validate_coordinates,
|
||||
_parse_linear_solver)
|
||||
from sympy.physics.mechanics.linearize import Linearizer
|
||||
from sympy.utilities.iterables import iterable
|
||||
|
||||
|
||||
__all__ = ['KanesMethod']
|
||||
|
||||
|
||||
class KanesMethod(_Methods):
|
||||
r"""Kane's method object.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
This object is used to do the "book-keeping" as you go through and form
|
||||
equations of motion in the way Kane presents in:
|
||||
Kane, T., Levinson, D. Dynamics Theory and Applications. 1985 McGraw-Hill
|
||||
|
||||
The attributes are for equations in the form [M] udot = forcing.
|
||||
|
||||
Attributes
|
||||
==========
|
||||
|
||||
q, u : Matrix
|
||||
Matrices of the generalized coordinates and speeds
|
||||
bodies : iterable
|
||||
Iterable of Particle and RigidBody objects in the system.
|
||||
loads : iterable
|
||||
Iterable of (Point, vector) or (ReferenceFrame, vector) tuples
|
||||
describing the forces on the system.
|
||||
auxiliary_eqs : Matrix
|
||||
If applicable, the set of auxiliary Kane's
|
||||
equations used to solve for non-contributing
|
||||
forces.
|
||||
mass_matrix : Matrix
|
||||
The system's dynamics mass matrix: [k_d; k_dnh]
|
||||
forcing : Matrix
|
||||
The system's dynamics forcing vector: -[f_d; f_dnh]
|
||||
mass_matrix_kin : Matrix
|
||||
The "mass matrix" for kinematic differential equations: k_kqdot
|
||||
forcing_kin : Matrix
|
||||
The forcing vector for kinematic differential equations: -(k_ku*u + f_k)
|
||||
mass_matrix_full : Matrix
|
||||
The "mass matrix" for the u's and q's with dynamics and kinematics
|
||||
forcing_full : Matrix
|
||||
The "forcing vector" for the u's and q's with dynamics and kinematics
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
frame : ReferenceFrame
|
||||
The inertial reference frame for the system.
|
||||
q_ind : iterable of dynamicsymbols
|
||||
Independent generalized coordinates.
|
||||
u_ind : iterable of dynamicsymbols
|
||||
Independent generalized speeds.
|
||||
kd_eqs : iterable of Expr, optional
|
||||
Kinematic differential equations, which linearly relate the generalized
|
||||
speeds to the time-derivatives of the generalized coordinates.
|
||||
q_dependent : iterable of dynamicsymbols, optional
|
||||
Dependent generalized coordinates.
|
||||
configuration_constraints : iterable of Expr, optional
|
||||
Constraints on the system's configuration, i.e. holonomic constraints.
|
||||
u_dependent : iterable of dynamicsymbols, optional
|
||||
Dependent generalized speeds.
|
||||
velocity_constraints : iterable of Expr, optional
|
||||
Constraints on the system's velocity, i.e. the combination of the
|
||||
nonholonomic constraints and the time-derivative of the holonomic
|
||||
constraints.
|
||||
acceleration_constraints : iterable of Expr, optional
|
||||
Constraints on the system's acceleration, by default these are the
|
||||
time-derivative of the velocity constraints.
|
||||
u_auxiliary : iterable of dynamicsymbols, optional
|
||||
Auxiliary generalized speeds.
|
||||
bodies : iterable of Particle and/or RigidBody, optional
|
||||
The particles and rigid bodies in the system.
|
||||
forcelist : iterable of tuple[Point | ReferenceFrame, Vector], optional
|
||||
Forces and torques applied on the system.
|
||||
explicit_kinematics : bool
|
||||
Boolean whether the mass matrices and forcing vectors should use the
|
||||
explicit form (default) or implicit form for kinematics.
|
||||
See the notes for more details.
|
||||
kd_eqs_solver : str, callable
|
||||
Method used to solve the kinematic differential equations. If a string
|
||||
is supplied, it should be a valid method that can be used with the
|
||||
:meth:`sympy.matrices.matrixbase.MatrixBase.solve`. If a callable is
|
||||
supplied, it should have the format ``f(A, rhs)``, where it solves the
|
||||
equations and returns the solution. The default utilizes LU solve. See
|
||||
the notes for more information.
|
||||
constraint_solver : str, callable
|
||||
Method used to solve the velocity constraints. If a string is
|
||||
supplied, it should be a valid method that can be used with the
|
||||
:meth:`sympy.matrices.matrixbase.MatrixBase.solve`. If a callable is
|
||||
supplied, it should have the format ``f(A, rhs)``, where it solves the
|
||||
equations and returns the solution. The default utilizes LU solve. See
|
||||
the notes for more information.
|
||||
|
||||
Notes
|
||||
=====
|
||||
|
||||
The mass matrices and forcing vectors related to kinematic equations
|
||||
are given in the explicit form by default. In other words, the kinematic
|
||||
mass matrix is $\mathbf{k_{k\dot{q}}} = \mathbf{I}$.
|
||||
In order to get the implicit form of those matrices/vectors, you can set the
|
||||
``explicit_kinematics`` attribute to ``False``. So $\mathbf{k_{k\dot{q}}}$
|
||||
is not necessarily an identity matrix. This can provide more compact
|
||||
equations for non-simple kinematics.
|
||||
|
||||
Two linear solvers can be supplied to ``KanesMethod``: one for solving the
|
||||
kinematic differential equations and one to solve the velocity constraints.
|
||||
Both of these sets of equations can be expressed as a linear system ``Ax = rhs``,
|
||||
which have to be solved in order to obtain the equations of motion.
|
||||
|
||||
The default solver ``'LU'``, which stands for LU solve, results relatively low
|
||||
number of operations. The weakness of this method is that it can result in zero
|
||||
division errors.
|
||||
|
||||
If zero divisions are encountered, a possible solver which may solve the problem
|
||||
is ``"CRAMER"``. This method uses Cramer's rule to solve the system. This method
|
||||
is slower and results in more operations than the default solver. However it only
|
||||
uses a single division by default per entry of the solution.
|
||||
|
||||
While a valid list of solvers can be found at
|
||||
:meth:`sympy.matrices.matrixbase.MatrixBase.solve`, it is also possible to supply a
|
||||
`callable`. This way it is possible to use a different solver routine. If the
|
||||
kinematic differential equations are not too complex it can be worth it to simplify
|
||||
the solution by using ``lambda A, b: simplify(Matrix.LUsolve(A, b))``. Another
|
||||
option solver one may use is :func:`sympy.solvers.solveset.linsolve`. This can be
|
||||
done using `lambda A, b: tuple(linsolve((A, b)))[0]`, where we select the first
|
||||
solution as our system should have only one unique solution.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
This is a simple example for a one degree of freedom translational
|
||||
spring-mass-damper.
|
||||
|
||||
In this example, we first need to do the kinematics.
|
||||
This involves creating generalized speeds and coordinates and their
|
||||
derivatives.
|
||||
Then we create a point and set its velocity in a frame.
|
||||
|
||||
>>> from sympy import symbols
|
||||
>>> from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame
|
||||
>>> from sympy.physics.mechanics import Point, Particle, KanesMethod
|
||||
>>> q, u = dynamicsymbols('q u')
|
||||
>>> qd, ud = dynamicsymbols('q u', 1)
|
||||
>>> m, c, k = symbols('m c k')
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> P = Point('P')
|
||||
>>> P.set_vel(N, u * N.x)
|
||||
|
||||
Next we need to arrange/store information in the way that KanesMethod
|
||||
requires. The kinematic differential equations should be an iterable of
|
||||
expressions. A list of forces/torques must be constructed, where each entry
|
||||
in the list is a (Point, Vector) or (ReferenceFrame, Vector) tuple, where
|
||||
the Vectors represent the Force or Torque.
|
||||
Next a particle needs to be created, and it needs to have a point and mass
|
||||
assigned to it.
|
||||
Finally, a list of all bodies and particles needs to be created.
|
||||
|
||||
>>> kd = [qd - u]
|
||||
>>> FL = [(P, (-k * q - c * u) * N.x)]
|
||||
>>> pa = Particle('pa', P, m)
|
||||
>>> BL = [pa]
|
||||
|
||||
Finally we can generate the equations of motion.
|
||||
First we create the KanesMethod object and supply an inertial frame,
|
||||
coordinates, generalized speeds, and the kinematic differential equations.
|
||||
Additional quantities such as configuration and motion constraints,
|
||||
dependent coordinates and speeds, and auxiliary speeds are also supplied
|
||||
here (see the online documentation).
|
||||
Next we form FR* and FR to complete: Fr + Fr* = 0.
|
||||
We have the equations of motion at this point.
|
||||
It makes sense to rearrange them though, so we calculate the mass matrix and
|
||||
the forcing terms, for E.o.M. in the form: [MM] udot = forcing, where MM is
|
||||
the mass matrix, udot is a vector of the time derivatives of the
|
||||
generalized speeds, and forcing is a vector representing "forcing" terms.
|
||||
|
||||
>>> KM = KanesMethod(N, q_ind=[q], u_ind=[u], kd_eqs=kd)
|
||||
>>> (fr, frstar) = KM.kanes_equations(BL, FL)
|
||||
>>> MM = KM.mass_matrix
|
||||
>>> forcing = KM.forcing
|
||||
>>> rhs = MM.inv() * forcing
|
||||
>>> rhs
|
||||
Matrix([[(-c*u(t) - k*q(t))/m]])
|
||||
>>> KM.linearize(A_and_B=True)[0]
|
||||
Matrix([
|
||||
[ 0, 1],
|
||||
[-k/m, -c/m]])
|
||||
|
||||
Please look at the documentation pages for more information on how to
|
||||
perform linearization and how to deal with dependent coordinates & speeds,
|
||||
and how do deal with bringing non-contributing forces into evidence.
|
||||
|
||||
"""
|
||||
|
||||
def __init__(self, frame, q_ind, u_ind, kd_eqs=None, q_dependent=None,
|
||||
configuration_constraints=None, u_dependent=None,
|
||||
velocity_constraints=None, acceleration_constraints=None,
|
||||
u_auxiliary=None, bodies=None, forcelist=None,
|
||||
explicit_kinematics=True, kd_eqs_solver='LU',
|
||||
constraint_solver='LU'):
|
||||
|
||||
"""Please read the online documentation. """
|
||||
if not q_ind:
|
||||
q_ind = [dynamicsymbols('dummy_q')]
|
||||
kd_eqs = [dynamicsymbols('dummy_kd')]
|
||||
|
||||
if not isinstance(frame, ReferenceFrame):
|
||||
raise TypeError('An inertial ReferenceFrame must be supplied')
|
||||
self._inertial = frame
|
||||
|
||||
self._fr = None
|
||||
self._frstar = None
|
||||
|
||||
self._forcelist = forcelist
|
||||
self._bodylist = bodies
|
||||
|
||||
self.explicit_kinematics = explicit_kinematics
|
||||
self._constraint_solver = constraint_solver
|
||||
self._initialize_vectors(q_ind, q_dependent, u_ind, u_dependent,
|
||||
u_auxiliary)
|
||||
_validate_coordinates(self.q, self.u)
|
||||
self._initialize_kindiffeq_matrices(kd_eqs, kd_eqs_solver)
|
||||
self._initialize_constraint_matrices(
|
||||
configuration_constraints, velocity_constraints,
|
||||
acceleration_constraints, constraint_solver)
|
||||
|
||||
def _initialize_vectors(self, q_ind, q_dep, u_ind, u_dep, u_aux):
|
||||
"""Initialize the coordinate and speed vectors."""
|
||||
|
||||
none_handler = lambda x: Matrix(x) if x else Matrix()
|
||||
|
||||
# Initialize generalized coordinates
|
||||
q_dep = none_handler(q_dep)
|
||||
if not iterable(q_ind):
|
||||
raise TypeError('Generalized coordinates must be an iterable.')
|
||||
if not iterable(q_dep):
|
||||
raise TypeError('Dependent coordinates must be an iterable.')
|
||||
q_ind = Matrix(q_ind)
|
||||
self._qdep = q_dep
|
||||
self._q = Matrix([q_ind, q_dep])
|
||||
self._qdot = self.q.diff(dynamicsymbols._t)
|
||||
|
||||
# Initialize generalized speeds
|
||||
u_dep = none_handler(u_dep)
|
||||
if not iterable(u_ind):
|
||||
raise TypeError('Generalized speeds must be an iterable.')
|
||||
if not iterable(u_dep):
|
||||
raise TypeError('Dependent speeds must be an iterable.')
|
||||
u_ind = Matrix(u_ind)
|
||||
self._udep = u_dep
|
||||
self._u = Matrix([u_ind, u_dep])
|
||||
self._udot = self.u.diff(dynamicsymbols._t)
|
||||
self._uaux = none_handler(u_aux)
|
||||
|
||||
def _initialize_constraint_matrices(self, config, vel, acc, linear_solver='LU'):
|
||||
"""Initializes constraint matrices."""
|
||||
linear_solver = _parse_linear_solver(linear_solver)
|
||||
# Define vector dimensions
|
||||
o = len(self.u)
|
||||
m = len(self._udep)
|
||||
p = o - m
|
||||
none_handler = lambda x: Matrix(x) if x else Matrix()
|
||||
|
||||
# Initialize configuration constraints
|
||||
config = none_handler(config)
|
||||
if len(self._qdep) != len(config):
|
||||
raise ValueError('There must be an equal number of dependent '
|
||||
'coordinates and configuration constraints.')
|
||||
self._f_h = none_handler(config)
|
||||
|
||||
# Initialize velocity and acceleration constraints
|
||||
vel = none_handler(vel)
|
||||
acc = none_handler(acc)
|
||||
if len(vel) != m:
|
||||
raise ValueError('There must be an equal number of dependent '
|
||||
'speeds and velocity constraints.')
|
||||
if acc and (len(acc) != m):
|
||||
raise ValueError('There must be an equal number of dependent '
|
||||
'speeds and acceleration constraints.')
|
||||
if vel:
|
||||
|
||||
# When calling kanes_equations, another class instance will be
|
||||
# created if auxiliary u's are present. In this case, the
|
||||
# computation of kinetic differential equation matrices will be
|
||||
# skipped as this was computed during the original KanesMethod
|
||||
# object, and the qd_u_map will not be available.
|
||||
if self._qdot_u_map is not None:
|
||||
vel = msubs(vel, self._qdot_u_map)
|
||||
self._k_nh, f_nh_neg = linear_eq_to_matrix(vel, self.u[:])
|
||||
self._f_nh = -f_nh_neg
|
||||
|
||||
# If no acceleration constraints given, calculate them.
|
||||
if not acc:
|
||||
_f_dnh = (self._k_nh.diff(dynamicsymbols._t) * self.u +
|
||||
self._f_nh.diff(dynamicsymbols._t))
|
||||
if self._qdot_u_map is not None:
|
||||
_f_dnh = msubs(_f_dnh, self._qdot_u_map)
|
||||
self._f_dnh = _f_dnh
|
||||
self._k_dnh = self._k_nh
|
||||
else:
|
||||
if self._qdot_u_map is not None:
|
||||
acc = msubs(acc, self._qdot_u_map)
|
||||
|
||||
self._k_dnh, f_dnh_neg = linear_eq_to_matrix(acc, self._udot[:])
|
||||
self._f_dnh = -f_dnh_neg
|
||||
# Form of non-holonomic constraints is B*u + C = 0.
|
||||
# We partition B into independent and dependent columns:
|
||||
# Ars is then -B_dep.inv() * B_ind, and it relates dependent speeds
|
||||
# to independent speeds as: udep = Ars*uind, neglecting the C term.
|
||||
B_ind = self._k_nh[:, :p]
|
||||
B_dep = self._k_nh[:, p:o]
|
||||
self._Ars = -linear_solver(B_dep, B_ind)
|
||||
else:
|
||||
self._f_nh = Matrix()
|
||||
self._k_nh = Matrix()
|
||||
self._f_dnh = Matrix()
|
||||
self._k_dnh = Matrix()
|
||||
self._Ars = Matrix()
|
||||
|
||||
def _initialize_kindiffeq_matrices(self, kdeqs, linear_solver='LU'):
|
||||
"""Initialize the kinematic differential equation matrices.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
kdeqs : sequence of sympy expressions
|
||||
Kinematic differential equations in the form of f(u,q',q,t) where
|
||||
f() = 0. The equations have to be linear in the time-derivatives of
|
||||
the generalized coordinates and in the generalized speeds.
|
||||
|
||||
"""
|
||||
linear_solver = _parse_linear_solver(linear_solver)
|
||||
if kdeqs:
|
||||
if len(self.q) != len(kdeqs):
|
||||
raise ValueError('There must be an equal number of kinematic '
|
||||
'differential equations and coordinates.')
|
||||
|
||||
u = self.u
|
||||
qdot = self._qdot
|
||||
|
||||
kdeqs = Matrix(kdeqs)
|
||||
|
||||
u_zero = dict.fromkeys(u, 0)
|
||||
uaux_zero = dict.fromkeys(self._uaux, 0)
|
||||
qdot_zero = dict.fromkeys(qdot, 0)
|
||||
|
||||
# Extract the linear coefficient matrices as per the following
|
||||
# equation:
|
||||
#
|
||||
# k_ku(q,t)*u(t) + k_kqdot(q,t)*q'(t) + f_k(q,t) = 0
|
||||
#
|
||||
k_ku = kdeqs.jacobian(u)
|
||||
k_kqdot = kdeqs.jacobian(qdot)
|
||||
f_k = kdeqs.xreplace(u_zero).xreplace(qdot_zero)
|
||||
|
||||
# The kinematic differential equations should be linear in both q'
|
||||
# and u so check for u and q' in the components.
|
||||
dy_syms = find_dynamicsymbols(k_ku.row_join(k_kqdot).row_join(f_k))
|
||||
nonlin_vars = [vari for vari in u[:] + qdot[:] if vari in dy_syms]
|
||||
if nonlin_vars:
|
||||
msg = ('The provided kinematic differential equations are '
|
||||
'nonlinear in {}. They must be linear in the '
|
||||
'generalized speeds and derivatives of the generalized '
|
||||
'coordinates.')
|
||||
raise ValueError(msg.format(nonlin_vars))
|
||||
|
||||
self._f_k_implicit = f_k.xreplace(uaux_zero)
|
||||
self._k_ku_implicit = k_ku.xreplace(uaux_zero)
|
||||
self._k_kqdot_implicit = k_kqdot
|
||||
|
||||
# Solve for q'(t) such that the coefficient matrices are now in
|
||||
# this form:
|
||||
#
|
||||
# k_kqdot^-1*k_ku*u(t) + I*q'(t) + k_kqdot^-1*f_k = 0
|
||||
#
|
||||
# NOTE : Solving the kinematic differential equations here is not
|
||||
# necessary and prevents the equations from being provided in fully
|
||||
# implicit form.
|
||||
f_k_explicit = linear_solver(k_kqdot, f_k)
|
||||
k_ku_explicit = linear_solver(k_kqdot, k_ku)
|
||||
self._qdot_u_map = dict(zip(qdot, -(k_ku_explicit*u + f_k_explicit)))
|
||||
|
||||
self._f_k = f_k_explicit.xreplace(uaux_zero)
|
||||
self._k_ku = k_ku_explicit.xreplace(uaux_zero)
|
||||
self._k_kqdot = eye(len(qdot))
|
||||
|
||||
else:
|
||||
self._qdot_u_map = None
|
||||
self._f_k_implicit = self._f_k = Matrix()
|
||||
self._k_ku_implicit = self._k_ku = Matrix()
|
||||
self._k_kqdot_implicit = self._k_kqdot = Matrix()
|
||||
|
||||
def _form_fr(self, fl):
|
||||
"""Form the generalized active force."""
|
||||
if fl is not None and (len(fl) == 0 or not iterable(fl)):
|
||||
raise ValueError('Force pairs must be supplied in an '
|
||||
'non-empty iterable or None.')
|
||||
|
||||
N = self._inertial
|
||||
# pull out relevant velocities for constructing partial velocities
|
||||
vel_list, f_list = _f_list_parser(fl, N)
|
||||
vel_list = [msubs(i, self._qdot_u_map) for i in vel_list]
|
||||
f_list = [msubs(i, self._qdot_u_map) for i in f_list]
|
||||
|
||||
# Fill Fr with dot product of partial velocities and forces
|
||||
o = len(self.u)
|
||||
b = len(f_list)
|
||||
FR = zeros(o, 1)
|
||||
partials = partial_velocity(vel_list, self.u, N)
|
||||
for i in range(o):
|
||||
FR[i] = sum(partials[j][i].dot(f_list[j]) for j in range(b))
|
||||
|
||||
# In case there are dependent speeds
|
||||
if self._udep:
|
||||
p = o - len(self._udep)
|
||||
FRtilde = FR[:p, 0]
|
||||
FRold = FR[p:o, 0]
|
||||
FRtilde += self._Ars.T * FRold
|
||||
FR = FRtilde
|
||||
|
||||
self._forcelist = fl
|
||||
self._fr = FR
|
||||
return FR
|
||||
|
||||
def _form_frstar(self, bl):
|
||||
"""Form the generalized inertia force."""
|
||||
|
||||
if not iterable(bl):
|
||||
raise TypeError('Bodies must be supplied in an iterable.')
|
||||
|
||||
t = dynamicsymbols._t
|
||||
N = self._inertial
|
||||
# Dicts setting things to zero
|
||||
udot_zero = dict.fromkeys(self._udot, 0)
|
||||
uaux_zero = dict.fromkeys(self._uaux, 0)
|
||||
uauxdot = [diff(i, t) for i in self._uaux]
|
||||
uauxdot_zero = dict.fromkeys(uauxdot, 0)
|
||||
# Dictionary of q' and q'' to u and u'
|
||||
q_ddot_u_map = {k.diff(t): v.diff(t).xreplace(
|
||||
self._qdot_u_map) for (k, v) in self._qdot_u_map.items()}
|
||||
q_ddot_u_map.update(self._qdot_u_map)
|
||||
|
||||
# Fill up the list of partials: format is a list with num elements
|
||||
# equal to number of entries in body list. Each of these elements is a
|
||||
# list - either of length 1 for the translational components of
|
||||
# particles or of length 2 for the translational and rotational
|
||||
# components of rigid bodies. The inner most list is the list of
|
||||
# partial velocities.
|
||||
def get_partial_velocity(body):
|
||||
if isinstance(body, RigidBody):
|
||||
vlist = [body.masscenter.vel(N), body.frame.ang_vel_in(N)]
|
||||
elif isinstance(body, Particle):
|
||||
vlist = [body.point.vel(N),]
|
||||
else:
|
||||
raise TypeError('The body list may only contain either '
|
||||
'RigidBody or Particle as list elements.')
|
||||
v = [msubs(vel, self._qdot_u_map) for vel in vlist]
|
||||
return partial_velocity(v, self.u, N)
|
||||
partials = [get_partial_velocity(body) for body in bl]
|
||||
|
||||
# Compute fr_star in two components:
|
||||
# fr_star = -(MM*u' + nonMM)
|
||||
o = len(self.u)
|
||||
MM = zeros(o, o)
|
||||
nonMM = zeros(o, 1)
|
||||
zero_uaux = lambda expr: msubs(expr, uaux_zero)
|
||||
zero_udot_uaux = lambda expr: msubs(msubs(expr, udot_zero), uaux_zero)
|
||||
for i, body in enumerate(bl):
|
||||
if isinstance(body, RigidBody):
|
||||
M = zero_uaux(body.mass)
|
||||
I = zero_uaux(body.central_inertia)
|
||||
vel = zero_uaux(body.masscenter.vel(N))
|
||||
omega = zero_uaux(body.frame.ang_vel_in(N))
|
||||
acc = zero_udot_uaux(body.masscenter.acc(N))
|
||||
inertial_force = (M.diff(t) * vel + M * acc)
|
||||
inertial_torque = zero_uaux((I.dt(body.frame).dot(omega)) +
|
||||
msubs(I.dot(body.frame.ang_acc_in(N)), udot_zero) +
|
||||
(omega.cross(I.dot(omega))))
|
||||
for j in range(o):
|
||||
tmp_vel = zero_uaux(partials[i][0][j])
|
||||
tmp_ang = zero_uaux(I.dot(partials[i][1][j]))
|
||||
for k in range(o):
|
||||
# translational
|
||||
MM[j, k] += M*tmp_vel.dot(partials[i][0][k])
|
||||
# rotational
|
||||
MM[j, k] += tmp_ang.dot(partials[i][1][k])
|
||||
nonMM[j] += inertial_force.dot(partials[i][0][j])
|
||||
nonMM[j] += inertial_torque.dot(partials[i][1][j])
|
||||
else:
|
||||
M = zero_uaux(body.mass)
|
||||
vel = zero_uaux(body.point.vel(N))
|
||||
acc = zero_udot_uaux(body.point.acc(N))
|
||||
inertial_force = (M.diff(t) * vel + M * acc)
|
||||
for j in range(o):
|
||||
temp = zero_uaux(partials[i][0][j])
|
||||
for k in range(o):
|
||||
MM[j, k] += M*temp.dot(partials[i][0][k])
|
||||
nonMM[j] += inertial_force.dot(partials[i][0][j])
|
||||
# Compose fr_star out of MM and nonMM
|
||||
MM = zero_uaux(msubs(MM, q_ddot_u_map))
|
||||
nonMM = msubs(msubs(nonMM, q_ddot_u_map),
|
||||
udot_zero, uauxdot_zero, uaux_zero)
|
||||
fr_star = -(MM * msubs(Matrix(self._udot), uauxdot_zero) + nonMM)
|
||||
|
||||
# If there are dependent speeds, we need to find fr_star_tilde
|
||||
if self._udep:
|
||||
p = o - len(self._udep)
|
||||
fr_star_ind = fr_star[:p, 0]
|
||||
fr_star_dep = fr_star[p:o, 0]
|
||||
fr_star = fr_star_ind + (self._Ars.T * fr_star_dep)
|
||||
# Apply the same to MM
|
||||
MMi = MM[:p, :]
|
||||
MMd = MM[p:o, :]
|
||||
MM = MMi + (self._Ars.T * MMd)
|
||||
# Apply the same to nonMM
|
||||
nonMM = nonMM[:p, :] + (self._Ars.T * nonMM[p:o, :])
|
||||
|
||||
self._bodylist = bl
|
||||
self._frstar = fr_star
|
||||
self._k_d = MM
|
||||
self._f_d = -(self._fr - nonMM)
|
||||
return fr_star
|
||||
|
||||
def to_linearizer(self, linear_solver='LU'):
|
||||
"""Returns an instance of the Linearizer class, initiated from the
|
||||
data in the KanesMethod class. This may be more desirable than using
|
||||
the linearize class method, as the Linearizer object will allow more
|
||||
efficient recalculation (i.e. about varying operating points).
|
||||
|
||||
Parameters
|
||||
==========
|
||||
linear_solver : str, callable
|
||||
Method used to solve the several symbolic linear systems of the
|
||||
form ``A*x=b`` in the linearization process. If a string is
|
||||
supplied, it should be a valid method that can be used with the
|
||||
:meth:`sympy.matrices.matrixbase.MatrixBase.solve`. If a callable is
|
||||
supplied, it should have the format ``x = f(A, b)``, where it
|
||||
solves the equations and returns the solution. The default is
|
||||
``'LU'`` which corresponds to SymPy's ``A.LUsolve(b)``.
|
||||
``LUsolve()`` is fast to compute but will often result in
|
||||
divide-by-zero and thus ``nan`` results.
|
||||
|
||||
Returns
|
||||
=======
|
||||
Linearizer
|
||||
An instantiated
|
||||
:class:`sympy.physics.mechanics.linearize.Linearizer`.
|
||||
|
||||
"""
|
||||
|
||||
if (self._fr is None) or (self._frstar is None):
|
||||
raise ValueError('Need to compute Fr, Fr* first.')
|
||||
|
||||
# Get required equation components. The Kane's method class breaks
|
||||
# these into pieces. Need to reassemble
|
||||
f_c = self._f_h
|
||||
if self._f_nh and self._k_nh:
|
||||
f_v = self._f_nh + self._k_nh*Matrix(self.u)
|
||||
else:
|
||||
f_v = Matrix()
|
||||
if self._f_dnh and self._k_dnh:
|
||||
f_a = self._f_dnh + self._k_dnh*Matrix(self._udot)
|
||||
else:
|
||||
f_a = Matrix()
|
||||
# Dicts to sub to zero, for splitting up expressions
|
||||
u_zero = dict.fromkeys(self.u, 0)
|
||||
ud_zero = dict.fromkeys(self._udot, 0)
|
||||
qd_zero = dict.fromkeys(self._qdot, 0)
|
||||
qd_u_zero = dict.fromkeys(Matrix([self._qdot, self.u]), 0)
|
||||
# Break the kinematic differential eqs apart into f_0 and f_1
|
||||
f_0 = msubs(self._f_k, u_zero) + self._k_kqdot*Matrix(self._qdot)
|
||||
f_1 = msubs(self._f_k, qd_zero) + self._k_ku*Matrix(self.u)
|
||||
# Break the dynamic differential eqs into f_2 and f_3
|
||||
f_2 = msubs(self._frstar, qd_u_zero)
|
||||
f_3 = msubs(self._frstar, ud_zero) + self._fr
|
||||
f_4 = zeros(len(f_2), 1)
|
||||
|
||||
# Get the required vector components
|
||||
q = self.q
|
||||
u = self.u
|
||||
if self._qdep:
|
||||
q_i = q[:-len(self._qdep)]
|
||||
else:
|
||||
q_i = q
|
||||
q_d = self._qdep
|
||||
if self._udep:
|
||||
u_i = u[:-len(self._udep)]
|
||||
else:
|
||||
u_i = u
|
||||
u_d = self._udep
|
||||
|
||||
# Form dictionary to set auxiliary speeds & their derivatives to 0.
|
||||
uaux = self._uaux
|
||||
uauxdot = uaux.diff(dynamicsymbols._t)
|
||||
uaux_zero = dict.fromkeys(Matrix([uaux, uauxdot]), 0)
|
||||
|
||||
# Checking for dynamic symbols outside the dynamic differential
|
||||
# equations; throws error if there is.
|
||||
sym_list = set(Matrix([q, self._qdot, u, self._udot, uaux, uauxdot]))
|
||||
if any(find_dynamicsymbols(i, sym_list) for i in [self._k_kqdot,
|
||||
self._k_ku, self._f_k, self._k_dnh, self._f_dnh, self._k_d]):
|
||||
raise ValueError('Cannot have dynamicsymbols outside dynamic \
|
||||
forcing vector.')
|
||||
|
||||
# Find all other dynamic symbols, forming the forcing vector r.
|
||||
# Sort r to make it canonical.
|
||||
r = list(find_dynamicsymbols(msubs(self._f_d, uaux_zero), sym_list))
|
||||
r.sort(key=default_sort_key)
|
||||
|
||||
# Check for any derivatives of variables in r that are also found in r.
|
||||
for i in r:
|
||||
if diff(i, dynamicsymbols._t) in r:
|
||||
raise ValueError('Cannot have derivatives of specified \
|
||||
quantities when linearizing forcing terms.')
|
||||
return Linearizer(f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a, q, u, q_i,
|
||||
q_d, u_i, u_d, r, linear_solver=linear_solver)
|
||||
|
||||
# TODO : Remove `new_method` after 1.1 has been released.
|
||||
def linearize(self, *, new_method=None, linear_solver='LU', **kwargs):
|
||||
""" Linearize the equations of motion about a symbolic operating point.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
new_method
|
||||
Deprecated, does nothing and will be removed.
|
||||
linear_solver : str, callable
|
||||
Method used to solve the several symbolic linear systems of the
|
||||
form ``A*x=b`` in the linearization process. If a string is
|
||||
supplied, it should be a valid method that can be used with the
|
||||
:meth:`sympy.matrices.matrixbase.MatrixBase.solve`. If a callable is
|
||||
supplied, it should have the format ``x = f(A, b)``, where it
|
||||
solves the equations and returns the solution. The default is
|
||||
``'LU'`` which corresponds to SymPy's ``A.LUsolve(b)``.
|
||||
``LUsolve()`` is fast to compute but will often result in
|
||||
divide-by-zero and thus ``nan`` results.
|
||||
**kwargs
|
||||
Extra keyword arguments are passed to
|
||||
:meth:`sympy.physics.mechanics.linearize.Linearizer.linearize`.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
If kwarg A_and_B is False (default), returns M, A, B, r for the
|
||||
linearized form, M*[q', u']^T = A*[q_ind, u_ind]^T + B*r.
|
||||
|
||||
If kwarg A_and_B is True, returns A, B, r for the linearized form
|
||||
dx = A*x + B*r, where x = [q_ind, u_ind]^T. Note that this is
|
||||
computationally intensive if there are many symbolic parameters. For
|
||||
this reason, it may be more desirable to use the default A_and_B=False,
|
||||
returning M, A, and B. Values may then be substituted in to these
|
||||
matrices, and the state space form found as
|
||||
A = P.T*M.inv()*A, B = P.T*M.inv()*B, where P = Linearizer.perm_mat.
|
||||
|
||||
In both cases, r is found as all dynamicsymbols in the equations of
|
||||
motion that are not part of q, u, q', or u'. They are sorted in
|
||||
canonical form.
|
||||
|
||||
The operating points may be also entered using the ``op_point`` kwarg.
|
||||
This takes a dictionary of {symbol: value}, or a an iterable of such
|
||||
dictionaries. The values may be numeric or symbolic. The more values
|
||||
you can specify beforehand, the faster this computation will run.
|
||||
|
||||
For more documentation, please see the ``Linearizer`` class.
|
||||
|
||||
"""
|
||||
|
||||
linearizer = self.to_linearizer(linear_solver=linear_solver)
|
||||
result = linearizer.linearize(**kwargs)
|
||||
return result + (linearizer.r,)
|
||||
|
||||
def kanes_equations(self, bodies=None, loads=None):
|
||||
""" Method to form Kane's equations, Fr + Fr* = 0.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
Returns (Fr, Fr*). In the case where auxiliary generalized speeds are
|
||||
present (say, s auxiliary speeds, o generalized speeds, and m motion
|
||||
constraints) the length of the returned vectors will be o - m + s in
|
||||
length. The first o - m equations will be the constrained Kane's
|
||||
equations, then the s auxiliary Kane's equations. These auxiliary
|
||||
equations can be accessed with the auxiliary_eqs property.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
bodies : iterable
|
||||
An iterable of all RigidBody's and Particle's in the system.
|
||||
A system must have at least one body.
|
||||
loads : iterable
|
||||
Takes in an iterable of (Particle, Vector) or (ReferenceFrame, Vector)
|
||||
tuples which represent the force at a point or torque on a frame.
|
||||
Must be either a non-empty iterable of tuples or None which corresponds
|
||||
to a system with no constraints.
|
||||
"""
|
||||
if bodies is None:
|
||||
bodies = self.bodies
|
||||
if loads is None and self._forcelist is not None:
|
||||
loads = self._forcelist
|
||||
if loads == []:
|
||||
loads = None
|
||||
if not self._k_kqdot:
|
||||
raise AttributeError('Create an instance of KanesMethod with '
|
||||
'kinematic differential equations to use this method.')
|
||||
fr = self._form_fr(loads)
|
||||
frstar = self._form_frstar(bodies)
|
||||
if self._uaux:
|
||||
if not self._udep:
|
||||
km = KanesMethod(self._inertial, self.q, self._uaux,
|
||||
u_auxiliary=self._uaux, constraint_solver=self._constraint_solver)
|
||||
else:
|
||||
km = KanesMethod(self._inertial, self.q, self._uaux,
|
||||
u_auxiliary=self._uaux, u_dependent=self._udep,
|
||||
velocity_constraints=(self._k_nh * self.u +
|
||||
self._f_nh),
|
||||
acceleration_constraints=(self._k_dnh * self._udot +
|
||||
self._f_dnh),
|
||||
constraint_solver=self._constraint_solver
|
||||
)
|
||||
km._qdot_u_map = self._qdot_u_map
|
||||
self._km = km
|
||||
fraux = km._form_fr(loads)
|
||||
frstaraux = km._form_frstar(bodies)
|
||||
self._aux_eq = fraux + frstaraux
|
||||
self._fr = fr.col_join(fraux)
|
||||
self._frstar = frstar.col_join(frstaraux)
|
||||
return (self._fr, self._frstar)
|
||||
|
||||
def _form_eoms(self):
|
||||
fr, frstar = self.kanes_equations(self.bodylist, self.forcelist)
|
||||
return fr + frstar
|
||||
|
||||
def rhs(self, inv_method=None):
|
||||
"""Returns the system's equations of motion in first order form. The
|
||||
output is the right hand side of::
|
||||
|
||||
x' = |q'| =: f(q, u, r, p, t)
|
||||
|u'|
|
||||
|
||||
The right hand side is what is needed by most numerical ODE
|
||||
integrators.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
inv_method : str
|
||||
The specific sympy inverse matrix calculation method to use. For a
|
||||
list of valid methods, see
|
||||
:meth:`~sympy.matrices.matrixbase.MatrixBase.inv`
|
||||
|
||||
"""
|
||||
rhs = zeros(len(self.q) + len(self.u), 1)
|
||||
kdes = self.kindiffdict()
|
||||
for i, q_i in enumerate(self.q):
|
||||
rhs[i] = kdes[q_i.diff()]
|
||||
|
||||
if inv_method is None:
|
||||
rhs[len(self.q):, 0] = self.mass_matrix.LUsolve(self.forcing)
|
||||
else:
|
||||
rhs[len(self.q):, 0] = (self.mass_matrix.inv(inv_method,
|
||||
try_block_diag=True) *
|
||||
self.forcing)
|
||||
|
||||
return rhs
|
||||
|
||||
def kindiffdict(self):
|
||||
"""Returns a dictionary mapping q' to u."""
|
||||
if not self._qdot_u_map:
|
||||
raise AttributeError('Create an instance of KanesMethod with '
|
||||
'kinematic differential equations to use this method.')
|
||||
return self._qdot_u_map
|
||||
|
||||
@property
|
||||
def auxiliary_eqs(self):
|
||||
"""A matrix containing the auxiliary equations."""
|
||||
if not self._fr or not self._frstar:
|
||||
raise ValueError('Need to compute Fr, Fr* first.')
|
||||
if not self._uaux:
|
||||
raise ValueError('No auxiliary speeds have been declared.')
|
||||
return self._aux_eq
|
||||
|
||||
@property
|
||||
def mass_matrix_kin(self):
|
||||
r"""The kinematic "mass matrix" $\mathbf{k_{k\dot{q}}}$ of the system."""
|
||||
return self._k_kqdot if self.explicit_kinematics else self._k_kqdot_implicit
|
||||
|
||||
@property
|
||||
def forcing_kin(self):
|
||||
"""The kinematic "forcing vector" of the system."""
|
||||
if self.explicit_kinematics:
|
||||
return -(self._k_ku * Matrix(self.u) + self._f_k)
|
||||
else:
|
||||
return -(self._k_ku_implicit * Matrix(self.u) + self._f_k_implicit)
|
||||
|
||||
@property
|
||||
def mass_matrix(self):
|
||||
"""The mass matrix of the system."""
|
||||
if not self._fr or not self._frstar:
|
||||
raise ValueError('Need to compute Fr, Fr* first.')
|
||||
return Matrix([self._k_d, self._k_dnh])
|
||||
|
||||
@property
|
||||
def forcing(self):
|
||||
"""The forcing vector of the system."""
|
||||
if not self._fr or not self._frstar:
|
||||
raise ValueError('Need to compute Fr, Fr* first.')
|
||||
return -Matrix([self._f_d, self._f_dnh])
|
||||
|
||||
@property
|
||||
def mass_matrix_full(self):
|
||||
"""The mass matrix of the system, augmented by the kinematic
|
||||
differential equations in explicit or implicit form."""
|
||||
if not self._fr or not self._frstar:
|
||||
raise ValueError('Need to compute Fr, Fr* first.')
|
||||
o, n = len(self.u), len(self.q)
|
||||
return (self.mass_matrix_kin.row_join(zeros(n, o))).col_join(
|
||||
zeros(o, n).row_join(self.mass_matrix))
|
||||
|
||||
@property
|
||||
def forcing_full(self):
|
||||
"""The forcing vector of the system, augmented by the kinematic
|
||||
differential equations in explicit or implicit form."""
|
||||
return Matrix([self.forcing_kin, self.forcing])
|
||||
|
||||
@property
|
||||
def q(self):
|
||||
return self._q
|
||||
|
||||
@property
|
||||
def u(self):
|
||||
return self._u
|
||||
|
||||
@property
|
||||
def bodylist(self):
|
||||
return self._bodylist
|
||||
|
||||
@property
|
||||
def forcelist(self):
|
||||
return self._forcelist
|
||||
|
||||
@property
|
||||
def bodies(self):
|
||||
return self._bodylist
|
||||
|
||||
@property
|
||||
def loads(self):
|
||||
return self._forcelist
|
||||
+512
@@ -0,0 +1,512 @@
|
||||
from sympy import diff, zeros, Matrix, eye, sympify
|
||||
from sympy.core.sorting import default_sort_key
|
||||
from sympy.physics.vector import dynamicsymbols, ReferenceFrame
|
||||
from sympy.physics.mechanics.method import _Methods
|
||||
from sympy.physics.mechanics.functions import (
|
||||
find_dynamicsymbols, msubs, _f_list_parser, _validate_coordinates)
|
||||
from sympy.physics.mechanics.linearize import Linearizer
|
||||
from sympy.utilities.iterables import iterable
|
||||
|
||||
__all__ = ['LagrangesMethod']
|
||||
|
||||
|
||||
class LagrangesMethod(_Methods):
|
||||
"""Lagrange's method object.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
This object generates the equations of motion in a two step procedure. The
|
||||
first step involves the initialization of LagrangesMethod by supplying the
|
||||
Lagrangian and the generalized coordinates, at the bare minimum. If there
|
||||
are any constraint equations, they can be supplied as keyword arguments.
|
||||
The Lagrange multipliers are automatically generated and are equal in
|
||||
number to the constraint equations. Similarly any non-conservative forces
|
||||
can be supplied in an iterable (as described below and also shown in the
|
||||
example) along with a ReferenceFrame. This is also discussed further in the
|
||||
__init__ method.
|
||||
|
||||
Attributes
|
||||
==========
|
||||
|
||||
q, u : Matrix
|
||||
Matrices of the generalized coordinates and speeds
|
||||
loads : iterable
|
||||
Iterable of (Point, vector) or (ReferenceFrame, vector) tuples
|
||||
describing the forces on the system.
|
||||
bodies : iterable
|
||||
Iterable containing the rigid bodies and particles of the system.
|
||||
mass_matrix : Matrix
|
||||
The system's mass matrix
|
||||
forcing : Matrix
|
||||
The system's forcing vector
|
||||
mass_matrix_full : Matrix
|
||||
The "mass matrix" for the qdot's, qdoubledot's, and the
|
||||
lagrange multipliers (lam)
|
||||
forcing_full : Matrix
|
||||
The forcing vector for the qdot's, qdoubledot's and
|
||||
lagrange multipliers (lam)
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
This is a simple example for a one degree of freedom translational
|
||||
spring-mass-damper.
|
||||
|
||||
In this example, we first need to do the kinematics.
|
||||
This involves creating generalized coordinates and their derivatives.
|
||||
Then we create a point and set its velocity in a frame.
|
||||
|
||||
>>> from sympy.physics.mechanics import LagrangesMethod, Lagrangian
|
||||
>>> from sympy.physics.mechanics import ReferenceFrame, Particle, Point
|
||||
>>> from sympy.physics.mechanics import dynamicsymbols
|
||||
>>> from sympy import symbols
|
||||
>>> q = dynamicsymbols('q')
|
||||
>>> qd = dynamicsymbols('q', 1)
|
||||
>>> m, k, b = symbols('m k b')
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> P = Point('P')
|
||||
>>> P.set_vel(N, qd * N.x)
|
||||
|
||||
We need to then prepare the information as required by LagrangesMethod to
|
||||
generate equations of motion.
|
||||
First we create the Particle, which has a point attached to it.
|
||||
Following this the lagrangian is created from the kinetic and potential
|
||||
energies.
|
||||
Then, an iterable of nonconservative forces/torques must be constructed,
|
||||
where each item is a (Point, Vector) or (ReferenceFrame, Vector) tuple,
|
||||
with the Vectors representing the nonconservative forces or torques.
|
||||
|
||||
>>> Pa = Particle('Pa', P, m)
|
||||
>>> Pa.potential_energy = k * q**2 / 2.0
|
||||
>>> L = Lagrangian(N, Pa)
|
||||
>>> fl = [(P, -b * qd * N.x)]
|
||||
|
||||
Finally we can generate the equations of motion.
|
||||
First we create the LagrangesMethod object. To do this one must supply
|
||||
the Lagrangian, and the generalized coordinates. The constraint equations,
|
||||
the forcelist, and the inertial frame may also be provided, if relevant.
|
||||
Next we generate Lagrange's equations of motion, such that:
|
||||
Lagrange's equations of motion = 0.
|
||||
We have the equations of motion at this point.
|
||||
|
||||
>>> l = LagrangesMethod(L, [q], forcelist = fl, frame = N)
|
||||
>>> print(l.form_lagranges_equations())
|
||||
Matrix([[b*Derivative(q(t), t) + 1.0*k*q(t) + m*Derivative(q(t), (t, 2))]])
|
||||
|
||||
We can also solve for the states using the 'rhs' method.
|
||||
|
||||
>>> print(l.rhs())
|
||||
Matrix([[Derivative(q(t), t)], [(-b*Derivative(q(t), t) - 1.0*k*q(t))/m]])
|
||||
|
||||
Please refer to the docstrings on each method for more details.
|
||||
"""
|
||||
|
||||
def __init__(self, Lagrangian, qs, forcelist=None, bodies=None, frame=None,
|
||||
hol_coneqs=None, nonhol_coneqs=None):
|
||||
"""Supply the following for the initialization of LagrangesMethod.
|
||||
|
||||
Lagrangian : Sympifyable
|
||||
|
||||
qs : array_like
|
||||
The generalized coordinates
|
||||
|
||||
hol_coneqs : array_like, optional
|
||||
The holonomic constraint equations
|
||||
|
||||
nonhol_coneqs : array_like, optional
|
||||
The nonholonomic constraint equations
|
||||
|
||||
forcelist : iterable, optional
|
||||
Takes an iterable of (Point, Vector) or (ReferenceFrame, Vector)
|
||||
tuples which represent the force at a point or torque on a frame.
|
||||
This feature is primarily to account for the nonconservative forces
|
||||
and/or moments.
|
||||
|
||||
bodies : iterable, optional
|
||||
Takes an iterable containing the rigid bodies and particles of the
|
||||
system.
|
||||
|
||||
frame : ReferenceFrame, optional
|
||||
Supply the inertial frame. This is used to determine the
|
||||
generalized forces due to non-conservative forces.
|
||||
"""
|
||||
|
||||
self._L = Matrix([sympify(Lagrangian)])
|
||||
self.eom = None
|
||||
self._m_cd = Matrix() # Mass Matrix of differentiated coneqs
|
||||
self._m_d = Matrix() # Mass Matrix of dynamic equations
|
||||
self._f_cd = Matrix() # Forcing part of the diff coneqs
|
||||
self._f_d = Matrix() # Forcing part of the dynamic equations
|
||||
self.lam_coeffs = Matrix() # The coeffecients of the multipliers
|
||||
|
||||
forcelist = forcelist if forcelist else []
|
||||
if not iterable(forcelist):
|
||||
raise TypeError('Force pairs must be supplied in an iterable.')
|
||||
self._forcelist = forcelist
|
||||
if frame and not isinstance(frame, ReferenceFrame):
|
||||
raise TypeError('frame must be a valid ReferenceFrame')
|
||||
self._bodies = bodies
|
||||
self.inertial = frame
|
||||
|
||||
self.lam_vec = Matrix()
|
||||
|
||||
self._term1 = Matrix()
|
||||
self._term2 = Matrix()
|
||||
self._term3 = Matrix()
|
||||
self._term4 = Matrix()
|
||||
|
||||
# Creating the qs, qdots and qdoubledots
|
||||
if not iterable(qs):
|
||||
raise TypeError('Generalized coordinates must be an iterable')
|
||||
self._q = Matrix(qs)
|
||||
self._qdots = self.q.diff(dynamicsymbols._t)
|
||||
self._qdoubledots = self._qdots.diff(dynamicsymbols._t)
|
||||
_validate_coordinates(self.q)
|
||||
|
||||
mat_build = lambda x: Matrix(x) if x else Matrix()
|
||||
hol_coneqs = mat_build(hol_coneqs)
|
||||
nonhol_coneqs = mat_build(nonhol_coneqs)
|
||||
self.coneqs = Matrix([hol_coneqs.diff(dynamicsymbols._t),
|
||||
nonhol_coneqs])
|
||||
self._hol_coneqs = hol_coneqs
|
||||
|
||||
def form_lagranges_equations(self):
|
||||
"""Method to form Lagrange's equations of motion.
|
||||
|
||||
Returns a vector of equations of motion using Lagrange's equations of
|
||||
the second kind.
|
||||
"""
|
||||
|
||||
qds = self._qdots
|
||||
qdd_zero = dict.fromkeys(self._qdoubledots, 0)
|
||||
n = len(self.q)
|
||||
|
||||
# Internally we represent the EOM as four terms:
|
||||
# EOM = term1 - term2 - term3 - term4 = 0
|
||||
|
||||
# First term
|
||||
self._term1 = self._L.jacobian(qds)
|
||||
self._term1 = self._term1.diff(dynamicsymbols._t).T
|
||||
|
||||
# Second term
|
||||
self._term2 = self._L.jacobian(self.q).T
|
||||
|
||||
# Third term
|
||||
if self.coneqs:
|
||||
coneqs = self.coneqs
|
||||
m = len(coneqs)
|
||||
# Creating the multipliers
|
||||
self.lam_vec = Matrix(dynamicsymbols('lam1:' + str(m + 1)))
|
||||
self.lam_coeffs = -coneqs.jacobian(qds)
|
||||
self._term3 = self.lam_coeffs.T * self.lam_vec
|
||||
# Extracting the coeffecients of the qdds from the diff coneqs
|
||||
diffconeqs = coneqs.diff(dynamicsymbols._t)
|
||||
self._m_cd = diffconeqs.jacobian(self._qdoubledots)
|
||||
# The remaining terms i.e. the 'forcing' terms in diff coneqs
|
||||
self._f_cd = -diffconeqs.subs(qdd_zero)
|
||||
else:
|
||||
self._term3 = zeros(n, 1)
|
||||
|
||||
# Fourth term
|
||||
if self.forcelist:
|
||||
N = self.inertial
|
||||
self._term4 = zeros(n, 1)
|
||||
for i, qd in enumerate(qds):
|
||||
flist = zip(*_f_list_parser(self.forcelist, N))
|
||||
self._term4[i] = sum(v.diff(qd, N).dot(f) for (v, f) in flist)
|
||||
else:
|
||||
self._term4 = zeros(n, 1)
|
||||
|
||||
# Form the dynamic mass and forcing matrices
|
||||
without_lam = self._term1 - self._term2 - self._term4
|
||||
self._m_d = without_lam.jacobian(self._qdoubledots)
|
||||
self._f_d = -without_lam.subs(qdd_zero)
|
||||
|
||||
# Form the EOM
|
||||
self.eom = without_lam - self._term3
|
||||
return self.eom
|
||||
|
||||
def _form_eoms(self):
|
||||
return self.form_lagranges_equations()
|
||||
|
||||
@property
|
||||
def mass_matrix(self):
|
||||
"""Returns the mass matrix, which is augmented by the Lagrange
|
||||
multipliers, if necessary.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
If the system is described by 'n' generalized coordinates and there are
|
||||
no constraint equations then an n X n matrix is returned.
|
||||
|
||||
If there are 'n' generalized coordinates and 'm' constraint equations
|
||||
have been supplied during initialization then an n X (n+m) matrix is
|
||||
returned. The (n + m - 1)th and (n + m)th columns contain the
|
||||
coefficients of the Lagrange multipliers.
|
||||
"""
|
||||
|
||||
if self.eom is None:
|
||||
raise ValueError('Need to compute the equations of motion first')
|
||||
if self.coneqs:
|
||||
return (self._m_d).row_join(self.lam_coeffs.T)
|
||||
else:
|
||||
return self._m_d
|
||||
|
||||
@property
|
||||
def mass_matrix_full(self):
|
||||
"""Augments the coefficients of qdots to the mass_matrix."""
|
||||
|
||||
if self.eom is None:
|
||||
raise ValueError('Need to compute the equations of motion first')
|
||||
n = len(self.q)
|
||||
m = len(self.coneqs)
|
||||
row1 = eye(n).row_join(zeros(n, n + m))
|
||||
row2 = zeros(n, n).row_join(self.mass_matrix)
|
||||
if self.coneqs:
|
||||
row3 = zeros(m, n).row_join(self._m_cd).row_join(zeros(m, m))
|
||||
return row1.col_join(row2).col_join(row3)
|
||||
else:
|
||||
return row1.col_join(row2)
|
||||
|
||||
@property
|
||||
def forcing(self):
|
||||
"""Returns the forcing vector from 'lagranges_equations' method."""
|
||||
|
||||
if self.eom is None:
|
||||
raise ValueError('Need to compute the equations of motion first')
|
||||
return self._f_d
|
||||
|
||||
@property
|
||||
def forcing_full(self):
|
||||
"""Augments qdots to the forcing vector above."""
|
||||
|
||||
if self.eom is None:
|
||||
raise ValueError('Need to compute the equations of motion first')
|
||||
if self.coneqs:
|
||||
return self._qdots.col_join(self.forcing).col_join(self._f_cd)
|
||||
else:
|
||||
return self._qdots.col_join(self.forcing)
|
||||
|
||||
def to_linearizer(self, q_ind=None, qd_ind=None, q_dep=None, qd_dep=None,
|
||||
linear_solver='LU'):
|
||||
"""Returns an instance of the Linearizer class, initiated from the data
|
||||
in the LagrangesMethod class. This may be more desirable than using the
|
||||
linearize class method, as the Linearizer object will allow more
|
||||
efficient recalculation (i.e. about varying operating points).
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
q_ind, qd_ind : array_like, optional
|
||||
The independent generalized coordinates and speeds.
|
||||
q_dep, qd_dep : array_like, optional
|
||||
The dependent generalized coordinates and speeds.
|
||||
linear_solver : str, callable
|
||||
Method used to solve the several symbolic linear systems of the
|
||||
form ``A*x=b`` in the linearization process. If a string is
|
||||
supplied, it should be a valid method that can be used with the
|
||||
:meth:`sympy.matrices.matrixbase.MatrixBase.solve`. If a callable is
|
||||
supplied, it should have the format ``x = f(A, b)``, where it
|
||||
solves the equations and returns the solution. The default is
|
||||
``'LU'`` which corresponds to SymPy's ``A.LUsolve(b)``.
|
||||
``LUsolve()`` is fast to compute but will often result in
|
||||
divide-by-zero and thus ``nan`` results.
|
||||
|
||||
Returns
|
||||
=======
|
||||
Linearizer
|
||||
An instantiated
|
||||
:class:`sympy.physics.mechanics.linearize.Linearizer`.
|
||||
|
||||
"""
|
||||
|
||||
# Compose vectors
|
||||
t = dynamicsymbols._t
|
||||
q = self.q
|
||||
u = self._qdots
|
||||
ud = u.diff(t)
|
||||
# Get vector of lagrange multipliers
|
||||
lams = self.lam_vec
|
||||
|
||||
mat_build = lambda x: Matrix(x) if x else Matrix()
|
||||
q_i = mat_build(q_ind)
|
||||
q_d = mat_build(q_dep)
|
||||
u_i = mat_build(qd_ind)
|
||||
u_d = mat_build(qd_dep)
|
||||
|
||||
# Compose general form equations
|
||||
f_c = self._hol_coneqs
|
||||
f_v = self.coneqs
|
||||
f_a = f_v.diff(t)
|
||||
f_0 = u
|
||||
f_1 = -u
|
||||
f_2 = self._term1
|
||||
f_3 = -(self._term2 + self._term4)
|
||||
f_4 = -self._term3
|
||||
|
||||
# Check that there are an appropriate number of independent and
|
||||
# dependent coordinates
|
||||
if len(q_d) != len(f_c) or len(u_d) != len(f_v):
|
||||
raise ValueError(("Must supply {:} dependent coordinates, and " +
|
||||
"{:} dependent speeds").format(len(f_c), len(f_v)))
|
||||
if set(Matrix([q_i, q_d])) != set(q):
|
||||
raise ValueError("Must partition q into q_ind and q_dep, with " +
|
||||
"no extra or missing symbols.")
|
||||
if set(Matrix([u_i, u_d])) != set(u):
|
||||
raise ValueError("Must partition qd into qd_ind and qd_dep, " +
|
||||
"with no extra or missing symbols.")
|
||||
|
||||
# Find all other dynamic symbols, forming the forcing vector r.
|
||||
# Sort r to make it canonical.
|
||||
insyms = set(Matrix([q, u, ud, lams]))
|
||||
r = list(find_dynamicsymbols(f_3, insyms))
|
||||
r.sort(key=default_sort_key)
|
||||
# Check for any derivatives of variables in r that are also found in r.
|
||||
for i in r:
|
||||
if diff(i, dynamicsymbols._t) in r:
|
||||
raise ValueError('Cannot have derivatives of specified \
|
||||
quantities when linearizing forcing terms.')
|
||||
|
||||
return Linearizer(f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a, q, u, q_i,
|
||||
q_d, u_i, u_d, r, lams, linear_solver=linear_solver)
|
||||
|
||||
def linearize(self, q_ind=None, qd_ind=None, q_dep=None, qd_dep=None,
|
||||
linear_solver='LU', **kwargs):
|
||||
"""Linearize the equations of motion about a symbolic operating point.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
linear_solver : str, callable
|
||||
Method used to solve the several symbolic linear systems of the
|
||||
form ``A*x=b`` in the linearization process. If a string is
|
||||
supplied, it should be a valid method that can be used with the
|
||||
:meth:`sympy.matrices.matrixbase.MatrixBase.solve`. If a callable is
|
||||
supplied, it should have the format ``x = f(A, b)``, where it
|
||||
solves the equations and returns the solution. The default is
|
||||
``'LU'`` which corresponds to SymPy's ``A.LUsolve(b)``.
|
||||
``LUsolve()`` is fast to compute but will often result in
|
||||
divide-by-zero and thus ``nan`` results.
|
||||
**kwargs
|
||||
Extra keyword arguments are passed to
|
||||
:meth:`sympy.physics.mechanics.linearize.Linearizer.linearize`.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
If kwarg A_and_B is False (default), returns M, A, B, r for the
|
||||
linearized form, M*[q', u']^T = A*[q_ind, u_ind]^T + B*r.
|
||||
|
||||
If kwarg A_and_B is True, returns A, B, r for the linearized form
|
||||
dx = A*x + B*r, where x = [q_ind, u_ind]^T. Note that this is
|
||||
computationally intensive if there are many symbolic parameters. For
|
||||
this reason, it may be more desirable to use the default A_and_B=False,
|
||||
returning M, A, and B. Values may then be substituted in to these
|
||||
matrices, and the state space form found as
|
||||
A = P.T*M.inv()*A, B = P.T*M.inv()*B, where P = Linearizer.perm_mat.
|
||||
|
||||
In both cases, r is found as all dynamicsymbols in the equations of
|
||||
motion that are not part of q, u, q', or u'. They are sorted in
|
||||
canonical form.
|
||||
|
||||
The operating points may be also entered using the ``op_point`` kwarg.
|
||||
This takes a dictionary of {symbol: value}, or a an iterable of such
|
||||
dictionaries. The values may be numeric or symbolic. The more values
|
||||
you can specify beforehand, the faster this computation will run.
|
||||
|
||||
For more documentation, please see the ``Linearizer`` class."""
|
||||
|
||||
linearizer = self.to_linearizer(q_ind, qd_ind, q_dep, qd_dep,
|
||||
linear_solver=linear_solver)
|
||||
result = linearizer.linearize(**kwargs)
|
||||
return result + (linearizer.r,)
|
||||
|
||||
def solve_multipliers(self, op_point=None, sol_type='dict'):
|
||||
"""Solves for the values of the lagrange multipliers symbolically at
|
||||
the specified operating point.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
op_point : dict or iterable of dicts, optional
|
||||
Point at which to solve at. The operating point is specified as
|
||||
a dictionary or iterable of dictionaries of {symbol: value}. The
|
||||
value may be numeric or symbolic itself.
|
||||
|
||||
sol_type : str, optional
|
||||
Solution return type. Valid options are:
|
||||
- 'dict': A dict of {symbol : value} (default)
|
||||
- 'Matrix': An ordered column matrix of the solution
|
||||
"""
|
||||
|
||||
# Determine number of multipliers
|
||||
k = len(self.lam_vec)
|
||||
if k == 0:
|
||||
raise ValueError("System has no lagrange multipliers to solve for.")
|
||||
# Compose dict of operating conditions
|
||||
if isinstance(op_point, dict):
|
||||
op_point_dict = op_point
|
||||
elif iterable(op_point):
|
||||
op_point_dict = {}
|
||||
for op in op_point:
|
||||
op_point_dict.update(op)
|
||||
elif op_point is None:
|
||||
op_point_dict = {}
|
||||
else:
|
||||
raise TypeError("op_point must be either a dictionary or an "
|
||||
"iterable of dictionaries.")
|
||||
# Compose the system to be solved
|
||||
mass_matrix = self.mass_matrix.col_join(-self.lam_coeffs.row_join(
|
||||
zeros(k, k)))
|
||||
force_matrix = self.forcing.col_join(self._f_cd)
|
||||
# Sub in the operating point
|
||||
mass_matrix = msubs(mass_matrix, op_point_dict)
|
||||
force_matrix = msubs(force_matrix, op_point_dict)
|
||||
# Solve for the multipliers
|
||||
sol_list = mass_matrix.LUsolve(-force_matrix)[-k:]
|
||||
if sol_type == 'dict':
|
||||
return dict(zip(self.lam_vec, sol_list))
|
||||
elif sol_type == 'Matrix':
|
||||
return Matrix(sol_list)
|
||||
else:
|
||||
raise ValueError("Unknown sol_type {:}.".format(sol_type))
|
||||
|
||||
def rhs(self, inv_method=None, **kwargs):
|
||||
"""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.matrixbase.MatrixBase.inv`
|
||||
"""
|
||||
|
||||
if inv_method is None:
|
||||
self._rhs = self.mass_matrix_full.LUsolve(self.forcing_full)
|
||||
else:
|
||||
self._rhs = (self.mass_matrix_full.inv(inv_method,
|
||||
try_block_diag=True) * self.forcing_full)
|
||||
return self._rhs
|
||||
|
||||
@property
|
||||
def q(self):
|
||||
return self._q
|
||||
|
||||
@property
|
||||
def u(self):
|
||||
return self._qdots
|
||||
|
||||
@property
|
||||
def bodies(self):
|
||||
return self._bodies
|
||||
|
||||
@property
|
||||
def forcelist(self):
|
||||
return self._forcelist
|
||||
|
||||
@property
|
||||
def loads(self):
|
||||
return self._forcelist
|
||||
+474
@@ -0,0 +1,474 @@
|
||||
__all__ = ['Linearizer']
|
||||
|
||||
from sympy import Matrix, eye, zeros
|
||||
from sympy.core.symbol import Dummy
|
||||
from sympy.utilities.iterables import flatten
|
||||
from sympy.physics.vector import dynamicsymbols
|
||||
from sympy.physics.mechanics.functions import msubs, _parse_linear_solver
|
||||
|
||||
from collections import namedtuple
|
||||
from collections.abc import Iterable
|
||||
|
||||
|
||||
class Linearizer:
|
||||
"""This object holds the general model form for a dynamic system. This
|
||||
model is used for computing the linearized form of the system, while
|
||||
properly dealing with constraints leading to dependent coordinates and
|
||||
speeds. The notation and method is described in [1]_.
|
||||
|
||||
Attributes
|
||||
==========
|
||||
|
||||
f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a : Matrix
|
||||
Matrices holding the general system form.
|
||||
q, u, r : Matrix
|
||||
Matrices holding the generalized coordinates, speeds, and
|
||||
input vectors.
|
||||
q_i, u_i : Matrix
|
||||
Matrices of the independent generalized coordinates and speeds.
|
||||
q_d, u_d : Matrix
|
||||
Matrices of the dependent generalized coordinates and speeds.
|
||||
perm_mat : Matrix
|
||||
Permutation matrix such that [q_ind, u_ind]^T = perm_mat*[q, u]^T
|
||||
|
||||
References
|
||||
==========
|
||||
|
||||
.. [1] D. L. Peterson, G. Gede, and M. Hubbard, "Symbolic linearization of
|
||||
equations of motion of constrained multibody systems," Multibody
|
||||
Syst Dyn, vol. 33, no. 2, pp. 143-161, Feb. 2015, doi:
|
||||
10.1007/s11044-014-9436-5.
|
||||
|
||||
"""
|
||||
|
||||
def __init__(self, f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a, q, u, q_i=None,
|
||||
q_d=None, u_i=None, u_d=None, r=None, lams=None,
|
||||
linear_solver='LU'):
|
||||
"""
|
||||
Parameters
|
||||
==========
|
||||
|
||||
f_0, f_1, f_2, f_3, f_4, f_c, f_v, f_a : array_like
|
||||
System of equations holding the general system form.
|
||||
Supply empty array or Matrix if the parameter
|
||||
does not exist.
|
||||
q : array_like
|
||||
The generalized coordinates.
|
||||
u : array_like
|
||||
The generalized speeds
|
||||
q_i, u_i : array_like, optional
|
||||
The independent generalized coordinates and speeds.
|
||||
q_d, u_d : array_like, optional
|
||||
The dependent generalized coordinates and speeds.
|
||||
r : array_like, optional
|
||||
The input variables.
|
||||
lams : array_like, optional
|
||||
The lagrange multipliers
|
||||
linear_solver : str, callable
|
||||
Method used to solve the several symbolic linear systems of the
|
||||
form ``A*x=b`` in the linearization process. If a string is
|
||||
supplied, it should be a valid method that can be used with the
|
||||
:meth:`sympy.matrices.matrixbase.MatrixBase.solve`. If a callable is
|
||||
supplied, it should have the format ``x = f(A, b)``, where it
|
||||
solves the equations and returns the solution. The default is
|
||||
``'LU'`` which corresponds to SymPy's ``A.LUsolve(b)``.
|
||||
``LUsolve()`` is fast to compute but will often result in
|
||||
divide-by-zero and thus ``nan`` results.
|
||||
|
||||
"""
|
||||
self.linear_solver = _parse_linear_solver(linear_solver)
|
||||
|
||||
# Generalized equation form
|
||||
self.f_0 = Matrix(f_0)
|
||||
self.f_1 = Matrix(f_1)
|
||||
self.f_2 = Matrix(f_2)
|
||||
self.f_3 = Matrix(f_3)
|
||||
self.f_4 = Matrix(f_4)
|
||||
self.f_c = Matrix(f_c)
|
||||
self.f_v = Matrix(f_v)
|
||||
self.f_a = Matrix(f_a)
|
||||
|
||||
# Generalized equation variables
|
||||
self.q = Matrix(q)
|
||||
self.u = Matrix(u)
|
||||
none_handler = lambda x: Matrix(x) if x else Matrix()
|
||||
self.q_i = none_handler(q_i)
|
||||
self.q_d = none_handler(q_d)
|
||||
self.u_i = none_handler(u_i)
|
||||
self.u_d = none_handler(u_d)
|
||||
self.r = none_handler(r)
|
||||
self.lams = none_handler(lams)
|
||||
|
||||
# Derivatives of generalized equation variables
|
||||
self._qd = self.q.diff(dynamicsymbols._t)
|
||||
self._ud = self.u.diff(dynamicsymbols._t)
|
||||
# If the user doesn't actually use generalized variables, and the
|
||||
# qd and u vectors have any intersecting variables, this can cause
|
||||
# problems. We'll fix this with some hackery, and Dummy variables
|
||||
dup_vars = set(self._qd).intersection(self.u)
|
||||
self._qd_dup = Matrix([var if var not in dup_vars else Dummy() for var
|
||||
in self._qd])
|
||||
|
||||
# Derive dimension terms
|
||||
l = len(self.f_c)
|
||||
m = len(self.f_v)
|
||||
n = len(self.q)
|
||||
o = len(self.u)
|
||||
s = len(self.r)
|
||||
k = len(self.lams)
|
||||
dims = namedtuple('dims', ['l', 'm', 'n', 'o', 's', 'k'])
|
||||
self._dims = dims(l, m, n, o, s, k)
|
||||
|
||||
self._Pq = None
|
||||
self._Pqi = None
|
||||
self._Pqd = None
|
||||
self._Pu = None
|
||||
self._Pui = None
|
||||
self._Pud = None
|
||||
self._C_0 = None
|
||||
self._C_1 = None
|
||||
self._C_2 = None
|
||||
self.perm_mat = None
|
||||
|
||||
self._setup_done = False
|
||||
|
||||
def _setup(self):
|
||||
# Calculations here only need to be run once. They are moved out of
|
||||
# the __init__ method to increase the speed of Linearizer creation.
|
||||
self._form_permutation_matrices()
|
||||
self._form_block_matrices()
|
||||
self._form_coefficient_matrices()
|
||||
self._setup_done = True
|
||||
|
||||
def _form_permutation_matrices(self):
|
||||
"""Form the permutation matrices Pq and Pu."""
|
||||
|
||||
# Extract dimension variables
|
||||
l, m, n, o, s, k = self._dims
|
||||
# Compute permutation matrices
|
||||
if n != 0:
|
||||
self._Pq = permutation_matrix(self.q, Matrix([self.q_i, self.q_d]))
|
||||
if l > 0:
|
||||
self._Pqi = self._Pq[:, :-l]
|
||||
self._Pqd = self._Pq[:, -l:]
|
||||
else:
|
||||
self._Pqi = self._Pq
|
||||
self._Pqd = Matrix()
|
||||
if o != 0:
|
||||
self._Pu = permutation_matrix(self.u, Matrix([self.u_i, self.u_d]))
|
||||
if m > 0:
|
||||
self._Pui = self._Pu[:, :-m]
|
||||
self._Pud = self._Pu[:, -m:]
|
||||
else:
|
||||
self._Pui = self._Pu
|
||||
self._Pud = Matrix()
|
||||
# Compute combination permutation matrix for computing A and B
|
||||
P_col1 = Matrix([self._Pqi, zeros(o + k, n - l)])
|
||||
P_col2 = Matrix([zeros(n, o - m), self._Pui, zeros(k, o - m)])
|
||||
if P_col1:
|
||||
if P_col2:
|
||||
self.perm_mat = P_col1.row_join(P_col2)
|
||||
else:
|
||||
self.perm_mat = P_col1
|
||||
else:
|
||||
self.perm_mat = P_col2
|
||||
|
||||
def _form_coefficient_matrices(self):
|
||||
"""Form the coefficient matrices C_0, C_1, and C_2."""
|
||||
|
||||
# Extract dimension variables
|
||||
l, m, n, o, s, k = self._dims
|
||||
# Build up the coefficient matrices C_0, C_1, and C_2
|
||||
# If there are configuration constraints (l > 0), form C_0 as normal.
|
||||
# If not, C_0 is I_(nxn). Note that this works even if n=0
|
||||
if l > 0:
|
||||
f_c_jac_q = self.f_c.jacobian(self.q)
|
||||
self._C_0 = (eye(n) - self._Pqd *
|
||||
self.linear_solver(f_c_jac_q*self._Pqd,
|
||||
f_c_jac_q))*self._Pqi
|
||||
else:
|
||||
self._C_0 = eye(n)
|
||||
# If there are motion constraints (m > 0), form C_1 and C_2 as normal.
|
||||
# If not, C_1 is 0, and C_2 is I_(oxo). Note that this works even if
|
||||
# o = 0.
|
||||
if m > 0:
|
||||
f_v_jac_u = self.f_v.jacobian(self.u)
|
||||
temp = f_v_jac_u * self._Pud
|
||||
if n != 0:
|
||||
f_v_jac_q = self.f_v.jacobian(self.q)
|
||||
self._C_1 = -self._Pud * self.linear_solver(temp, f_v_jac_q)
|
||||
else:
|
||||
self._C_1 = zeros(o, n)
|
||||
self._C_2 = (eye(o) - self._Pud *
|
||||
self.linear_solver(temp, f_v_jac_u))*self._Pui
|
||||
else:
|
||||
self._C_1 = zeros(o, n)
|
||||
self._C_2 = eye(o)
|
||||
|
||||
def _form_block_matrices(self):
|
||||
"""Form the block matrices for composing M, A, and B."""
|
||||
|
||||
# Extract dimension variables
|
||||
l, m, n, o, s, k = self._dims
|
||||
# Block Matrix Definitions. These are only defined if under certain
|
||||
# conditions. If undefined, an empty matrix is used instead
|
||||
if n != 0:
|
||||
self._M_qq = self.f_0.jacobian(self._qd)
|
||||
self._A_qq = -(self.f_0 + self.f_1).jacobian(self.q)
|
||||
else:
|
||||
self._M_qq = Matrix()
|
||||
self._A_qq = Matrix()
|
||||
if n != 0 and m != 0:
|
||||
self._M_uqc = self.f_a.jacobian(self._qd_dup)
|
||||
self._A_uqc = -self.f_a.jacobian(self.q)
|
||||
else:
|
||||
self._M_uqc = Matrix()
|
||||
self._A_uqc = Matrix()
|
||||
if n != 0 and o - m + k != 0:
|
||||
self._M_uqd = self.f_3.jacobian(self._qd_dup)
|
||||
self._A_uqd = -(self.f_2 + self.f_3 + self.f_4).jacobian(self.q)
|
||||
else:
|
||||
self._M_uqd = Matrix()
|
||||
self._A_uqd = Matrix()
|
||||
if o != 0 and m != 0:
|
||||
self._M_uuc = self.f_a.jacobian(self._ud)
|
||||
self._A_uuc = -self.f_a.jacobian(self.u)
|
||||
else:
|
||||
self._M_uuc = Matrix()
|
||||
self._A_uuc = Matrix()
|
||||
if o != 0 and o - m + k != 0:
|
||||
self._M_uud = self.f_2.jacobian(self._ud)
|
||||
self._A_uud = -(self.f_2 + self.f_3).jacobian(self.u)
|
||||
else:
|
||||
self._M_uud = Matrix()
|
||||
self._A_uud = Matrix()
|
||||
if o != 0 and n != 0:
|
||||
self._A_qu = -self.f_1.jacobian(self.u)
|
||||
else:
|
||||
self._A_qu = Matrix()
|
||||
if k != 0 and o - m + k != 0:
|
||||
self._M_uld = self.f_4.jacobian(self.lams)
|
||||
else:
|
||||
self._M_uld = Matrix()
|
||||
if s != 0 and o - m + k != 0:
|
||||
self._B_u = -self.f_3.jacobian(self.r)
|
||||
else:
|
||||
self._B_u = Matrix()
|
||||
|
||||
def linearize(self, op_point=None, A_and_B=False, simplify=False):
|
||||
"""Linearize the system about the operating point. Note that
|
||||
q_op, u_op, qd_op, ud_op must satisfy the equations of motion.
|
||||
These may be either symbolic or numeric.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
op_point : dict or iterable of dicts, optional
|
||||
Dictionary or iterable of dictionaries containing the operating
|
||||
point conditions for all or a subset of the generalized
|
||||
coordinates, generalized speeds, and time derivatives of the
|
||||
generalized speeds. These will be substituted into the linearized
|
||||
system before the linearization is complete. Leave set to ``None``
|
||||
if you want the operating point to be an arbitrary set of symbols.
|
||||
Note that any reduction in symbols (whether substituted for numbers
|
||||
or expressions with a common parameter) will result in faster
|
||||
runtime.
|
||||
A_and_B : bool, optional
|
||||
If A_and_B=False (default), (M, A, B) is returned and of
|
||||
A_and_B=True, (A, B) is returned. See below.
|
||||
simplify : bool, optional
|
||||
Determines if returned values are simplified before return.
|
||||
For large expressions this may be time consuming. Default is False.
|
||||
|
||||
Returns
|
||||
=======
|
||||
M, A, B : Matrices, ``A_and_B=False``
|
||||
Matrices from the implicit form:
|
||||
``[M]*[q', u']^T = [A]*[q_ind, u_ind]^T + [B]*r``
|
||||
A, B : Matrices, ``A_and_B=True``
|
||||
Matrices from the explicit form:
|
||||
``[q_ind', u_ind']^T = [A]*[q_ind, u_ind]^T + [B]*r``
|
||||
|
||||
Notes
|
||||
=====
|
||||
|
||||
Note that the process of solving with A_and_B=True is computationally
|
||||
intensive if there are many symbolic parameters. For this reason, it
|
||||
may be more desirable to use the default A_and_B=False, returning M, A,
|
||||
and B. More values may then be substituted in to these matrices later
|
||||
on. The state space form can then be found as A = P.T*M.LUsolve(A), B =
|
||||
P.T*M.LUsolve(B), where P = Linearizer.perm_mat.
|
||||
|
||||
"""
|
||||
|
||||
# Run the setup if needed:
|
||||
if not self._setup_done:
|
||||
self._setup()
|
||||
|
||||
# Compose dict of operating conditions
|
||||
if isinstance(op_point, dict):
|
||||
op_point_dict = op_point
|
||||
elif isinstance(op_point, Iterable):
|
||||
op_point_dict = {}
|
||||
for op in op_point:
|
||||
op_point_dict.update(op)
|
||||
else:
|
||||
op_point_dict = {}
|
||||
|
||||
# Extract dimension variables
|
||||
l, m, n, o, s, k = self._dims
|
||||
|
||||
# Rename terms to shorten expressions
|
||||
M_qq = self._M_qq
|
||||
M_uqc = self._M_uqc
|
||||
M_uqd = self._M_uqd
|
||||
M_uuc = self._M_uuc
|
||||
M_uud = self._M_uud
|
||||
M_uld = self._M_uld
|
||||
A_qq = self._A_qq
|
||||
A_uqc = self._A_uqc
|
||||
A_uqd = self._A_uqd
|
||||
A_qu = self._A_qu
|
||||
A_uuc = self._A_uuc
|
||||
A_uud = self._A_uud
|
||||
B_u = self._B_u
|
||||
C_0 = self._C_0
|
||||
C_1 = self._C_1
|
||||
C_2 = self._C_2
|
||||
|
||||
# Build up Mass Matrix
|
||||
# |M_qq 0_nxo 0_nxk|
|
||||
# M = |M_uqc M_uuc 0_mxk|
|
||||
# |M_uqd M_uud M_uld|
|
||||
if o != 0:
|
||||
col2 = Matrix([zeros(n, o), M_uuc, M_uud])
|
||||
if k != 0:
|
||||
col3 = Matrix([zeros(n + m, k), M_uld])
|
||||
if n != 0:
|
||||
col1 = Matrix([M_qq, M_uqc, M_uqd])
|
||||
if o != 0 and k != 0:
|
||||
M = col1.row_join(col2).row_join(col3)
|
||||
elif o != 0:
|
||||
M = col1.row_join(col2)
|
||||
else:
|
||||
M = col1
|
||||
elif k != 0:
|
||||
M = col2.row_join(col3)
|
||||
else:
|
||||
M = col2
|
||||
M_eq = msubs(M, op_point_dict)
|
||||
|
||||
# Build up state coefficient matrix A
|
||||
# |(A_qq + A_qu*C_1)*C_0 A_qu*C_2|
|
||||
# A = |(A_uqc + A_uuc*C_1)*C_0 A_uuc*C_2|
|
||||
# |(A_uqd + A_uud*C_1)*C_0 A_uud*C_2|
|
||||
# Col 1 is only defined if n != 0
|
||||
if n != 0:
|
||||
r1c1 = A_qq
|
||||
if o != 0:
|
||||
r1c1 += (A_qu * C_1)
|
||||
r1c1 = r1c1 * C_0
|
||||
if m != 0:
|
||||
r2c1 = A_uqc
|
||||
if o != 0:
|
||||
r2c1 += (A_uuc * C_1)
|
||||
r2c1 = r2c1 * C_0
|
||||
else:
|
||||
r2c1 = Matrix()
|
||||
if o - m + k != 0:
|
||||
r3c1 = A_uqd
|
||||
if o != 0:
|
||||
r3c1 += (A_uud * C_1)
|
||||
r3c1 = r3c1 * C_0
|
||||
else:
|
||||
r3c1 = Matrix()
|
||||
col1 = Matrix([r1c1, r2c1, r3c1])
|
||||
else:
|
||||
col1 = Matrix()
|
||||
# Col 2 is only defined if o != 0
|
||||
if o != 0:
|
||||
if n != 0:
|
||||
r1c2 = A_qu * C_2
|
||||
else:
|
||||
r1c2 = Matrix()
|
||||
if m != 0:
|
||||
r2c2 = A_uuc * C_2
|
||||
else:
|
||||
r2c2 = Matrix()
|
||||
if o - m + k != 0:
|
||||
r3c2 = A_uud * C_2
|
||||
else:
|
||||
r3c2 = Matrix()
|
||||
col2 = Matrix([r1c2, r2c2, r3c2])
|
||||
else:
|
||||
col2 = Matrix()
|
||||
if col1:
|
||||
if col2:
|
||||
Amat = col1.row_join(col2)
|
||||
else:
|
||||
Amat = col1
|
||||
else:
|
||||
Amat = col2
|
||||
Amat_eq = msubs(Amat, op_point_dict)
|
||||
|
||||
# Build up the B matrix if there are forcing variables
|
||||
# |0_(n + m)xs|
|
||||
# B = |B_u |
|
||||
if s != 0 and o - m + k != 0:
|
||||
Bmat = zeros(n + m, s).col_join(B_u)
|
||||
Bmat_eq = msubs(Bmat, op_point_dict)
|
||||
else:
|
||||
Bmat_eq = Matrix()
|
||||
|
||||
# kwarg A_and_B indicates to return A, B for forming the equation
|
||||
# dx = [A]x + [B]r, where x = [q_indnd, u_indnd]^T,
|
||||
if A_and_B:
|
||||
A_cont = self.perm_mat.T * self.linear_solver(M_eq, Amat_eq)
|
||||
if Bmat_eq:
|
||||
B_cont = self.perm_mat.T * self.linear_solver(M_eq, Bmat_eq)
|
||||
else:
|
||||
# Bmat = Matrix([]), so no need to sub
|
||||
B_cont = Bmat_eq
|
||||
if simplify:
|
||||
A_cont.simplify()
|
||||
B_cont.simplify()
|
||||
return A_cont, B_cont
|
||||
# Otherwise return M, A, B for forming the equation
|
||||
# [M]dx = [A]x + [B]r, where x = [q, u]^T
|
||||
else:
|
||||
if simplify:
|
||||
M_eq.simplify()
|
||||
Amat_eq.simplify()
|
||||
Bmat_eq.simplify()
|
||||
return M_eq, Amat_eq, Bmat_eq
|
||||
|
||||
|
||||
def permutation_matrix(orig_vec, per_vec):
|
||||
"""Compute the permutation matrix to change order of
|
||||
orig_vec into order of per_vec.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
orig_vec : array_like
|
||||
Symbols in original ordering.
|
||||
per_vec : array_like
|
||||
Symbols in new ordering.
|
||||
|
||||
Returns
|
||||
=======
|
||||
|
||||
p_matrix : Matrix
|
||||
Permutation matrix such that orig_vec == (p_matrix * per_vec).
|
||||
"""
|
||||
if not isinstance(orig_vec, (list, tuple)):
|
||||
orig_vec = flatten(orig_vec)
|
||||
if not isinstance(per_vec, (list, tuple)):
|
||||
per_vec = flatten(per_vec)
|
||||
if set(orig_vec) != set(per_vec):
|
||||
raise ValueError("orig_vec and per_vec must be the same length, "
|
||||
"and contain the same symbols.")
|
||||
ind_list = [orig_vec.index(i) for i in per_vec]
|
||||
p_matrix = zeros(len(orig_vec))
|
||||
for i, j in enumerate(ind_list):
|
||||
p_matrix[i, j] = 1
|
||||
return p_matrix
|
||||
+177
@@ -0,0 +1,177 @@
|
||||
from abc import ABC
|
||||
from collections import namedtuple
|
||||
from sympy.physics.mechanics.body_base import BodyBase
|
||||
from sympy.physics.vector import Vector, ReferenceFrame, Point
|
||||
|
||||
__all__ = ['LoadBase', 'Force', 'Torque']
|
||||
|
||||
|
||||
class LoadBase(ABC, namedtuple('LoadBase', ['location', 'vector'])):
|
||||
"""Abstract base class for the various loading types."""
|
||||
|
||||
def __add__(self, other):
|
||||
raise TypeError(f"unsupported operand type(s) for +: "
|
||||
f"'{self.__class__.__name__}' and "
|
||||
f"'{other.__class__.__name__}'")
|
||||
|
||||
def __mul__(self, other):
|
||||
raise TypeError(f"unsupported operand type(s) for *: "
|
||||
f"'{self.__class__.__name__}' and "
|
||||
f"'{other.__class__.__name__}'")
|
||||
|
||||
__radd__ = __add__
|
||||
__rmul__ = __mul__
|
||||
|
||||
|
||||
class Force(LoadBase):
|
||||
"""Force acting upon a point.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
A force is a vector that is bound to a line of action. This class stores
|
||||
both a point, which lies on the line of action, and the vector. A tuple can
|
||||
also be used, with the location as the first entry and the vector as second
|
||||
entry.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
A force of magnitude 2 along N.x acting on a point Po can be created as
|
||||
follows:
|
||||
|
||||
>>> from sympy.physics.mechanics import Point, ReferenceFrame, Force
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> Po = Point('Po')
|
||||
>>> Force(Po, 2 * N.x)
|
||||
(Po, 2*N.x)
|
||||
|
||||
If a body is supplied, then the center of mass of that body is used.
|
||||
|
||||
>>> from sympy.physics.mechanics import Particle
|
||||
>>> P = Particle('P', point=Po)
|
||||
>>> Force(P, 2 * N.x)
|
||||
(Po, 2*N.x)
|
||||
|
||||
"""
|
||||
|
||||
def __new__(cls, point, force):
|
||||
if isinstance(point, BodyBase):
|
||||
point = point.masscenter
|
||||
if not isinstance(point, Point):
|
||||
raise TypeError('Force location should be a Point.')
|
||||
if not isinstance(force, Vector):
|
||||
raise TypeError('Force vector should be a Vector.')
|
||||
return super().__new__(cls, point, force)
|
||||
|
||||
def __repr__(self):
|
||||
return (f'{self.__class__.__name__}(point={self.point}, '
|
||||
f'force={self.force})')
|
||||
|
||||
@property
|
||||
def point(self):
|
||||
return self.location
|
||||
|
||||
@property
|
||||
def force(self):
|
||||
return self.vector
|
||||
|
||||
|
||||
class Torque(LoadBase):
|
||||
"""Torque acting upon a frame.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
A torque is a free vector that is acting on a reference frame, which is
|
||||
associated with a rigid body. This class stores both the frame and the
|
||||
vector. A tuple can also be used, with the location as the first item and
|
||||
the vector as second item.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
A torque of magnitude 2 about N.x acting on a frame N can be created as
|
||||
follows:
|
||||
|
||||
>>> from sympy.physics.mechanics import ReferenceFrame, Torque
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> Torque(N, 2 * N.x)
|
||||
(N, 2*N.x)
|
||||
|
||||
If a body is supplied, then the frame fixed to that body is used.
|
||||
|
||||
>>> from sympy.physics.mechanics import RigidBody
|
||||
>>> rb = RigidBody('rb', frame=N)
|
||||
>>> Torque(rb, 2 * N.x)
|
||||
(N, 2*N.x)
|
||||
|
||||
"""
|
||||
|
||||
def __new__(cls, frame, torque):
|
||||
if isinstance(frame, BodyBase):
|
||||
frame = frame.frame
|
||||
if not isinstance(frame, ReferenceFrame):
|
||||
raise TypeError('Torque location should be a ReferenceFrame.')
|
||||
if not isinstance(torque, Vector):
|
||||
raise TypeError('Torque vector should be a Vector.')
|
||||
return super().__new__(cls, frame, torque)
|
||||
|
||||
def __repr__(self):
|
||||
return (f'{self.__class__.__name__}(frame={self.frame}, '
|
||||
f'torque={self.torque})')
|
||||
|
||||
@property
|
||||
def frame(self):
|
||||
return self.location
|
||||
|
||||
@property
|
||||
def torque(self):
|
||||
return self.vector
|
||||
|
||||
|
||||
def gravity(acceleration, *bodies):
|
||||
"""
|
||||
Returns a list of gravity forces given the acceleration
|
||||
due to gravity and any number of particles or rigidbodies.
|
||||
|
||||
Example
|
||||
=======
|
||||
|
||||
>>> from sympy.physics.mechanics import ReferenceFrame, Particle, RigidBody
|
||||
>>> from sympy.physics.mechanics.loads import gravity
|
||||
>>> from sympy import symbols
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> g = symbols('g')
|
||||
>>> P = Particle('P')
|
||||
>>> B = RigidBody('B')
|
||||
>>> gravity(g*N.y, P, B)
|
||||
[(P_masscenter, P_mass*g*N.y),
|
||||
(B_masscenter, B_mass*g*N.y)]
|
||||
|
||||
"""
|
||||
|
||||
gravity_force = []
|
||||
for body in bodies:
|
||||
if not isinstance(body, BodyBase):
|
||||
raise TypeError(f'{type(body)} is not a body type')
|
||||
gravity_force.append(Force(body.masscenter, body.mass * acceleration))
|
||||
return gravity_force
|
||||
|
||||
|
||||
def _parse_load(load):
|
||||
"""Helper function to parse loads and convert tuples to load objects."""
|
||||
if isinstance(load, LoadBase):
|
||||
return load
|
||||
elif isinstance(load, tuple):
|
||||
if len(load) != 2:
|
||||
raise ValueError(f'Load {load} should have a length of 2.')
|
||||
if isinstance(load[0], Point):
|
||||
return Force(load[0], load[1])
|
||||
elif isinstance(load[0], ReferenceFrame):
|
||||
return Torque(load[0], load[1])
|
||||
else:
|
||||
raise ValueError(f'Load not recognized. The load location {load[0]}'
|
||||
f' should either be a Point or a ReferenceFrame.')
|
||||
raise TypeError(f'Load type {type(load)} not recognized as a load. It '
|
||||
f'should be a Force, Torque or tuple.')
|
||||
+39
@@ -0,0 +1,39 @@
|
||||
from abc import ABC, abstractmethod
|
||||
|
||||
class _Methods(ABC):
|
||||
"""Abstract Base Class for all methods."""
|
||||
|
||||
@abstractmethod
|
||||
def q(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def u(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def bodies(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def loads(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def mass_matrix(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def forcing(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def mass_matrix_full(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def forcing_full(self):
|
||||
pass
|
||||
|
||||
def _form_eoms(self):
|
||||
raise NotImplementedError("Subclasses must implement this.")
|
||||
+230
@@ -0,0 +1,230 @@
|
||||
#!/usr/bin/env python
|
||||
"""This module contains some sample symbolic models used for testing and
|
||||
examples."""
|
||||
|
||||
# Internal imports
|
||||
from sympy.core import backend as sm
|
||||
import sympy.physics.mechanics as me
|
||||
|
||||
|
||||
def multi_mass_spring_damper(n=1, apply_gravity=False,
|
||||
apply_external_forces=False):
|
||||
r"""Returns a system containing the symbolic equations of motion and
|
||||
associated variables for a simple multi-degree of freedom point mass,
|
||||
spring, damper system with optional gravitational and external
|
||||
specified forces. For example, a two mass system under the influence of
|
||||
gravity and external forces looks like:
|
||||
|
||||
::
|
||||
|
||||
----------------
|
||||
| | | | g
|
||||
\ | | | V
|
||||
k0 / --- c0 |
|
||||
| | | x0, v0
|
||||
--------- V
|
||||
| m0 | -----
|
||||
--------- |
|
||||
| | | |
|
||||
\ v | | |
|
||||
k1 / f0 --- c1 |
|
||||
| | | x1, v1
|
||||
--------- V
|
||||
| m1 | -----
|
||||
---------
|
||||
| f1
|
||||
V
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
n : integer
|
||||
The number of masses in the serial chain.
|
||||
apply_gravity : boolean
|
||||
If true, gravity will be applied to each mass.
|
||||
apply_external_forces : boolean
|
||||
If true, a time varying external force will be applied to each mass.
|
||||
|
||||
Returns
|
||||
=======
|
||||
|
||||
kane : sympy.physics.mechanics.kane.KanesMethod
|
||||
A KanesMethod object.
|
||||
|
||||
"""
|
||||
|
||||
mass = sm.symbols('m:{}'.format(n))
|
||||
stiffness = sm.symbols('k:{}'.format(n))
|
||||
damping = sm.symbols('c:{}'.format(n))
|
||||
|
||||
acceleration_due_to_gravity = sm.symbols('g')
|
||||
|
||||
coordinates = me.dynamicsymbols('x:{}'.format(n))
|
||||
speeds = me.dynamicsymbols('v:{}'.format(n))
|
||||
specifieds = me.dynamicsymbols('f:{}'.format(n))
|
||||
|
||||
ceiling = me.ReferenceFrame('N')
|
||||
origin = me.Point('origin')
|
||||
origin.set_vel(ceiling, 0)
|
||||
|
||||
points = [origin]
|
||||
kinematic_equations = []
|
||||
particles = []
|
||||
forces = []
|
||||
|
||||
for i in range(n):
|
||||
|
||||
center = points[-1].locatenew('center{}'.format(i),
|
||||
coordinates[i] * ceiling.x)
|
||||
center.set_vel(ceiling, points[-1].vel(ceiling) +
|
||||
speeds[i] * ceiling.x)
|
||||
points.append(center)
|
||||
|
||||
block = me.Particle('block{}'.format(i), center, mass[i])
|
||||
|
||||
kinematic_equations.append(speeds[i] - coordinates[i].diff())
|
||||
|
||||
total_force = (-stiffness[i] * coordinates[i] -
|
||||
damping[i] * speeds[i])
|
||||
try:
|
||||
total_force += (stiffness[i + 1] * coordinates[i + 1] +
|
||||
damping[i + 1] * speeds[i + 1])
|
||||
except IndexError: # no force from below on last mass
|
||||
pass
|
||||
|
||||
if apply_gravity:
|
||||
total_force += mass[i] * acceleration_due_to_gravity
|
||||
|
||||
if apply_external_forces:
|
||||
total_force += specifieds[i]
|
||||
|
||||
forces.append((center, total_force * ceiling.x))
|
||||
|
||||
particles.append(block)
|
||||
|
||||
kane = me.KanesMethod(ceiling, q_ind=coordinates, u_ind=speeds,
|
||||
kd_eqs=kinematic_equations)
|
||||
kane.kanes_equations(particles, forces)
|
||||
|
||||
return kane
|
||||
|
||||
|
||||
def n_link_pendulum_on_cart(n=1, cart_force=True, joint_torques=False):
|
||||
r"""Returns the system containing the symbolic first order equations of
|
||||
motion for a 2D n-link pendulum on a sliding cart under the influence of
|
||||
gravity.
|
||||
|
||||
::
|
||||
|
||||
|
|
||||
o y v
|
||||
\ 0 ^ g
|
||||
\ |
|
||||
--\-|----
|
||||
| \| |
|
||||
F-> | o --|---> x
|
||||
| |
|
||||
---------
|
||||
o o
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
n : integer
|
||||
The number of links in the pendulum.
|
||||
cart_force : boolean, default=True
|
||||
If true an external specified lateral force is applied to the cart.
|
||||
joint_torques : boolean, default=False
|
||||
If true joint torques will be added as specified inputs at each
|
||||
joint.
|
||||
|
||||
Returns
|
||||
=======
|
||||
|
||||
kane : sympy.physics.mechanics.kane.KanesMethod
|
||||
A KanesMethod object.
|
||||
|
||||
Notes
|
||||
=====
|
||||
|
||||
The degrees of freedom of the system are n + 1, i.e. one for each
|
||||
pendulum link and one for the lateral motion of the cart.
|
||||
|
||||
M x' = F, where x = [u0, ..., un+1, q0, ..., qn+1]
|
||||
|
||||
The joint angles are all defined relative to the ground where the x axis
|
||||
defines the ground line and the y axis points up. The joint torques are
|
||||
applied between each adjacent link and the between the cart and the
|
||||
lower link where a positive torque corresponds to positive angle.
|
||||
|
||||
"""
|
||||
if n <= 0:
|
||||
raise ValueError('The number of links must be a positive integer.')
|
||||
|
||||
q = me.dynamicsymbols('q:{}'.format(n + 1))
|
||||
u = me.dynamicsymbols('u:{}'.format(n + 1))
|
||||
|
||||
if joint_torques is True:
|
||||
T = me.dynamicsymbols('T1:{}'.format(n + 1))
|
||||
|
||||
m = sm.symbols('m:{}'.format(n + 1))
|
||||
l = sm.symbols('l:{}'.format(n))
|
||||
g, t = sm.symbols('g t')
|
||||
|
||||
I = me.ReferenceFrame('I')
|
||||
O = me.Point('O')
|
||||
O.set_vel(I, 0)
|
||||
|
||||
P0 = me.Point('P0')
|
||||
P0.set_pos(O, q[0] * I.x)
|
||||
P0.set_vel(I, u[0] * I.x)
|
||||
Pa0 = me.Particle('Pa0', P0, m[0])
|
||||
|
||||
frames = [I]
|
||||
points = [P0]
|
||||
particles = [Pa0]
|
||||
forces = [(P0, -m[0] * g * I.y)]
|
||||
kindiffs = [q[0].diff(t) - u[0]]
|
||||
|
||||
if cart_force is True or joint_torques is True:
|
||||
specified = []
|
||||
else:
|
||||
specified = None
|
||||
|
||||
for i in range(n):
|
||||
Bi = I.orientnew('B{}'.format(i), 'Axis', [q[i + 1], I.z])
|
||||
Bi.set_ang_vel(I, u[i + 1] * I.z)
|
||||
frames.append(Bi)
|
||||
|
||||
Pi = points[-1].locatenew('P{}'.format(i + 1), l[i] * Bi.y)
|
||||
Pi.v2pt_theory(points[-1], I, Bi)
|
||||
points.append(Pi)
|
||||
|
||||
Pai = me.Particle('Pa' + str(i + 1), Pi, m[i + 1])
|
||||
particles.append(Pai)
|
||||
|
||||
forces.append((Pi, -m[i + 1] * g * I.y))
|
||||
|
||||
if joint_torques is True:
|
||||
|
||||
specified.append(T[i])
|
||||
|
||||
if i == 0:
|
||||
forces.append((I, -T[i] * I.z))
|
||||
|
||||
if i == n - 1:
|
||||
forces.append((Bi, T[i] * I.z))
|
||||
else:
|
||||
forces.append((Bi, T[i] * I.z - T[i + 1] * I.z))
|
||||
|
||||
kindiffs.append(q[i + 1].diff(t) - u[i + 1])
|
||||
|
||||
if cart_force is True:
|
||||
F = me.dynamicsymbols('F')
|
||||
forces.append((P0, F * I.x))
|
||||
specified.append(F)
|
||||
|
||||
kane = me.KanesMethod(I, q_ind=q, u_ind=u, kd_eqs=kindiffs)
|
||||
kane.kanes_equations(particles, forces)
|
||||
|
||||
return kane
|
||||
+209
@@ -0,0 +1,209 @@
|
||||
from sympy import S
|
||||
from sympy.physics.vector import cross, dot
|
||||
from sympy.physics.mechanics.body_base import BodyBase
|
||||
from sympy.physics.mechanics.inertia import inertia_of_point_mass
|
||||
from sympy.utilities.exceptions import sympy_deprecation_warning
|
||||
|
||||
__all__ = ['Particle']
|
||||
|
||||
|
||||
class Particle(BodyBase):
|
||||
"""A particle.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
Particles have a non-zero mass and lack spatial extension; they take up no
|
||||
space.
|
||||
|
||||
Values need to be supplied on initialization, but can be changed later.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
name : str
|
||||
Name of particle
|
||||
point : Point
|
||||
A physics/mechanics Point which represents the position, velocity, and
|
||||
acceleration of this Particle
|
||||
mass : Sympifyable
|
||||
A SymPy expression representing the Particle's mass
|
||||
potential_energy : Sympifyable
|
||||
The potential energy of the Particle.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
>>> from sympy.physics.mechanics import Particle, Point
|
||||
>>> from sympy import Symbol
|
||||
>>> po = Point('po')
|
||||
>>> m = Symbol('m')
|
||||
>>> pa = Particle('pa', po, m)
|
||||
>>> # Or you could change these later
|
||||
>>> pa.mass = m
|
||||
>>> pa.point = po
|
||||
|
||||
"""
|
||||
point = BodyBase.masscenter
|
||||
|
||||
def __init__(self, name, point=None, mass=None):
|
||||
super().__init__(name, point, mass)
|
||||
|
||||
def linear_momentum(self, frame):
|
||||
"""Linear momentum of the particle.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
The linear momentum L, of a particle P, with respect to frame N is
|
||||
given by:
|
||||
|
||||
L = m * v
|
||||
|
||||
where m is the mass of the particle, and v is the velocity of the
|
||||
particle in the frame N.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
frame : ReferenceFrame
|
||||
The frame in which linear momentum is desired.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
>>> from sympy.physics.mechanics import Particle, Point, ReferenceFrame
|
||||
>>> from sympy.physics.mechanics import dynamicsymbols
|
||||
>>> from sympy.physics.vector import init_vprinting
|
||||
>>> init_vprinting(pretty_print=False)
|
||||
>>> m, v = dynamicsymbols('m v')
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> P = Point('P')
|
||||
>>> A = Particle('A', P, m)
|
||||
>>> P.set_vel(N, v * N.x)
|
||||
>>> A.linear_momentum(N)
|
||||
m*v*N.x
|
||||
|
||||
"""
|
||||
|
||||
return self.mass * self.point.vel(frame)
|
||||
|
||||
def angular_momentum(self, point, frame):
|
||||
"""Angular momentum of the particle about the point.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
The angular momentum H, about some point O of a particle, P, is given
|
||||
by:
|
||||
|
||||
``H = cross(r, m * v)``
|
||||
|
||||
where r is the position vector from point O to the particle P, m is
|
||||
the mass of the particle, and v is the velocity of the particle in
|
||||
the inertial frame, N.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
point : Point
|
||||
The point about which angular momentum of the particle is desired.
|
||||
|
||||
frame : ReferenceFrame
|
||||
The frame in which angular momentum is desired.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
>>> from sympy.physics.mechanics import Particle, Point, ReferenceFrame
|
||||
>>> from sympy.physics.mechanics import dynamicsymbols
|
||||
>>> from sympy.physics.vector import init_vprinting
|
||||
>>> init_vprinting(pretty_print=False)
|
||||
>>> m, v, r = dynamicsymbols('m v r')
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> O = Point('O')
|
||||
>>> A = O.locatenew('A', r * N.x)
|
||||
>>> P = Particle('P', A, m)
|
||||
>>> P.point.set_vel(N, v * N.y)
|
||||
>>> P.angular_momentum(O, N)
|
||||
m*r*v*N.z
|
||||
|
||||
"""
|
||||
|
||||
return cross(self.point.pos_from(point),
|
||||
self.mass * self.point.vel(frame))
|
||||
|
||||
def kinetic_energy(self, frame):
|
||||
"""Kinetic energy of the particle.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
The kinetic energy, T, of a particle, P, is given by:
|
||||
|
||||
``T = 1/2 (dot(m * v, v))``
|
||||
|
||||
where m is the mass of particle P, and v is the velocity of the
|
||||
particle in the supplied ReferenceFrame.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
frame : ReferenceFrame
|
||||
The Particle's velocity is typically defined with respect to
|
||||
an inertial frame but any relevant frame in which the velocity is
|
||||
known can be supplied.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
>>> from sympy.physics.mechanics import Particle, Point, ReferenceFrame
|
||||
>>> from sympy import symbols
|
||||
>>> m, v, r = symbols('m v r')
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> O = Point('O')
|
||||
>>> P = Particle('P', O, m)
|
||||
>>> P.point.set_vel(N, v * N.y)
|
||||
>>> P.kinetic_energy(N)
|
||||
m*v**2/2
|
||||
|
||||
"""
|
||||
|
||||
return S.Half * self.mass * dot(self.point.vel(frame),
|
||||
self.point.vel(frame))
|
||||
|
||||
def set_potential_energy(self, scalar):
|
||||
sympy_deprecation_warning(
|
||||
"""
|
||||
The sympy.physics.mechanics.Particle.set_potential_energy()
|
||||
method is deprecated. Instead use
|
||||
|
||||
P.potential_energy = scalar
|
||||
""",
|
||||
deprecated_since_version="1.5",
|
||||
active_deprecations_target="deprecated-set-potential-energy",
|
||||
)
|
||||
self.potential_energy = scalar
|
||||
|
||||
def parallel_axis(self, point, frame):
|
||||
"""Returns an inertia dyadic of the particle with respect to another
|
||||
point and frame.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
point : sympy.physics.vector.Point
|
||||
The point to express the inertia dyadic about.
|
||||
frame : sympy.physics.vector.ReferenceFrame
|
||||
The reference frame used to construct the dyadic.
|
||||
|
||||
Returns
|
||||
=======
|
||||
|
||||
inertia : sympy.physics.vector.Dyadic
|
||||
The inertia dyadic of the particle expressed about the provided
|
||||
point and frame.
|
||||
|
||||
"""
|
||||
return inertia_of_point_mass(self.mass, self.point.pos_from(point),
|
||||
frame)
|
||||
+688
@@ -0,0 +1,688 @@
|
||||
"""Implementations of pathways for use by actuators."""
|
||||
|
||||
from abc import ABC, abstractmethod
|
||||
|
||||
from sympy.core.singleton import S
|
||||
from sympy.physics.mechanics.loads import Force
|
||||
from sympy.physics.mechanics.wrapping_geometry import WrappingGeometryBase
|
||||
from sympy.physics.vector import Point, dynamicsymbols
|
||||
|
||||
|
||||
__all__ = ['PathwayBase', 'LinearPathway', 'ObstacleSetPathway',
|
||||
'WrappingPathway']
|
||||
|
||||
|
||||
class PathwayBase(ABC):
|
||||
"""Abstract base class for all pathway classes to inherit from.
|
||||
|
||||
Notes
|
||||
=====
|
||||
|
||||
Instances of this class cannot be directly instantiated by users. However,
|
||||
it can be used to created custom pathway types through subclassing.
|
||||
|
||||
"""
|
||||
|
||||
def __init__(self, *attachments):
|
||||
"""Initializer for ``PathwayBase``."""
|
||||
self.attachments = attachments
|
||||
|
||||
@property
|
||||
def attachments(self):
|
||||
"""The pair of points defining a pathway's ends."""
|
||||
return self._attachments
|
||||
|
||||
@attachments.setter
|
||||
def attachments(self, attachments):
|
||||
if hasattr(self, '_attachments'):
|
||||
msg = (
|
||||
f'Can\'t set attribute `attachments` to {repr(attachments)} '
|
||||
f'as it is immutable.'
|
||||
)
|
||||
raise AttributeError(msg)
|
||||
if len(attachments) != 2:
|
||||
msg = (
|
||||
f'Value {repr(attachments)} passed to `attachments` was an '
|
||||
f'iterable of length {len(attachments)}, must be an iterable '
|
||||
f'of length 2.'
|
||||
)
|
||||
raise ValueError(msg)
|
||||
for i, point in enumerate(attachments):
|
||||
if not isinstance(point, Point):
|
||||
msg = (
|
||||
f'Value {repr(point)} passed to `attachments` at index '
|
||||
f'{i} was of type {type(point)}, must be {Point}.'
|
||||
)
|
||||
raise TypeError(msg)
|
||||
self._attachments = tuple(attachments)
|
||||
|
||||
@property
|
||||
@abstractmethod
|
||||
def length(self):
|
||||
"""An expression representing the pathway's length."""
|
||||
pass
|
||||
|
||||
@property
|
||||
@abstractmethod
|
||||
def extension_velocity(self):
|
||||
"""An expression representing the pathway's extension velocity."""
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def to_loads(self, force):
|
||||
"""Loads required by the equations of motion method classes.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
``KanesMethod`` requires a list of ``Point``-``Vector`` tuples to be
|
||||
passed to the ``loads`` parameters of its ``kanes_equations`` method
|
||||
when constructing the equations of motion. This method acts as a
|
||||
utility to produce the correctly-structred pairs of points and vectors
|
||||
required so that these can be easily concatenated with other items in
|
||||
the list of loads and passed to ``KanesMethod.kanes_equations``. These
|
||||
loads are also in the correct form to also be passed to the other
|
||||
equations of motion method classes, e.g. ``LagrangesMethod``.
|
||||
|
||||
"""
|
||||
pass
|
||||
|
||||
def __repr__(self):
|
||||
"""Default representation of a pathway."""
|
||||
attachments = ', '.join(str(a) for a in self.attachments)
|
||||
return f'{self.__class__.__name__}({attachments})'
|
||||
|
||||
|
||||
class LinearPathway(PathwayBase):
|
||||
"""Linear pathway between a pair of attachment points.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
A linear pathway forms a straight-line segment between two points and is
|
||||
the simplest pathway that can be formed. It will not interact with any
|
||||
other objects in the system, i.e. a ``LinearPathway`` will intersect other
|
||||
objects to ensure that the path between its two ends (its attachments) is
|
||||
the shortest possible.
|
||||
|
||||
A linear pathway is made up of two points that can move relative to each
|
||||
other, and a pair of equal and opposite forces acting on the points. If the
|
||||
positive time-varying Euclidean distance between the two points is defined,
|
||||
then the "extension velocity" is the time derivative of this distance. The
|
||||
extension velocity is positive when the two points are moving away from
|
||||
each other and negative when moving closer to each other. The direction for
|
||||
the force acting on either point is determined by constructing a unit
|
||||
vector directed from the other point to this point. This establishes a sign
|
||||
convention such that a positive force magnitude tends to push the points
|
||||
apart. The following diagram shows the positive force sense and the
|
||||
distance between the points::
|
||||
|
||||
P Q
|
||||
o<--- F --->o
|
||||
| |
|
||||
|<--l(t)--->|
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
>>> from sympy.physics.mechanics import LinearPathway
|
||||
|
||||
To construct a pathway, two points are required to be passed to the
|
||||
``attachments`` parameter as a ``tuple``.
|
||||
|
||||
>>> from sympy.physics.mechanics import Point
|
||||
>>> pA, pB = Point('pA'), Point('pB')
|
||||
>>> linear_pathway = LinearPathway(pA, pB)
|
||||
>>> linear_pathway
|
||||
LinearPathway(pA, pB)
|
||||
|
||||
The pathway created above isn't very interesting without the positions and
|
||||
velocities of its attachment points being described. Without this its not
|
||||
possible to describe how the pathway moves, i.e. its length or its
|
||||
extension velocity.
|
||||
|
||||
>>> from sympy.physics.mechanics import ReferenceFrame
|
||||
>>> from sympy.physics.vector import dynamicsymbols
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> q = dynamicsymbols('q')
|
||||
>>> pB.set_pos(pA, q*N.x)
|
||||
>>> pB.pos_from(pA)
|
||||
q(t)*N.x
|
||||
|
||||
A pathway's length can be accessed via its ``length`` attribute.
|
||||
|
||||
>>> linear_pathway.length
|
||||
sqrt(q(t)**2)
|
||||
|
||||
Note how what appears to be an overly-complex expression is returned. This
|
||||
is actually required as it ensures that a pathway's length is always
|
||||
positive.
|
||||
|
||||
A pathway's extension velocity can be accessed similarly via its
|
||||
``extension_velocity`` attribute.
|
||||
|
||||
>>> linear_pathway.extension_velocity
|
||||
sqrt(q(t)**2)*Derivative(q(t), t)/q(t)
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
attachments : tuple[Point, Point]
|
||||
Pair of ``Point`` objects between which the linear pathway spans.
|
||||
Constructor expects two points to be passed, e.g.
|
||||
``LinearPathway(Point('pA'), Point('pB'))``. More or fewer points will
|
||||
cause an error to be thrown.
|
||||
|
||||
"""
|
||||
|
||||
def __init__(self, *attachments):
|
||||
"""Initializer for ``LinearPathway``.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
attachments : Point
|
||||
Pair of ``Point`` objects between which the linear pathway spans.
|
||||
Constructor expects two points to be passed, e.g.
|
||||
``LinearPathway(Point('pA'), Point('pB'))``. More or fewer points
|
||||
will cause an error to be thrown.
|
||||
|
||||
"""
|
||||
super().__init__(*attachments)
|
||||
|
||||
@property
|
||||
def length(self):
|
||||
"""Exact analytical expression for the pathway's length."""
|
||||
return _point_pair_length(*self.attachments)
|
||||
|
||||
@property
|
||||
def extension_velocity(self):
|
||||
"""Exact analytical expression for the pathway's extension velocity."""
|
||||
return _point_pair_extension_velocity(*self.attachments)
|
||||
|
||||
def to_loads(self, force):
|
||||
"""Loads required by the equations of motion method classes.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
``KanesMethod`` requires a list of ``Point``-``Vector`` tuples to be
|
||||
passed to the ``loads`` parameters of its ``kanes_equations`` method
|
||||
when constructing the equations of motion. This method acts as a
|
||||
utility to produce the correctly-structred pairs of points and vectors
|
||||
required so that these can be easily concatenated with other items in
|
||||
the list of loads and passed to ``KanesMethod.kanes_equations``. These
|
||||
loads are also in the correct form to also be passed to the other
|
||||
equations of motion method classes, e.g. ``LagrangesMethod``.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
The below example shows how to generate the loads produced in a linear
|
||||
actuator that produces an expansile force ``F``. First, create a linear
|
||||
actuator between two points separated by the coordinate ``q`` in the
|
||||
``x`` direction of the global frame ``N``.
|
||||
|
||||
>>> from sympy.physics.mechanics import (LinearPathway, Point,
|
||||
... ReferenceFrame)
|
||||
>>> from sympy.physics.vector import dynamicsymbols
|
||||
>>> q = dynamicsymbols('q')
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> pA, pB = Point('pA'), Point('pB')
|
||||
>>> pB.set_pos(pA, q*N.x)
|
||||
>>> linear_pathway = LinearPathway(pA, pB)
|
||||
|
||||
Now create a symbol ``F`` to describe the magnitude of the (expansile)
|
||||
force that will be produced along the pathway. The list of loads that
|
||||
``KanesMethod`` requires can be produced by calling the pathway's
|
||||
``to_loads`` method with ``F`` passed as the only argument.
|
||||
|
||||
>>> from sympy import symbols
|
||||
>>> F = symbols('F')
|
||||
>>> linear_pathway.to_loads(F)
|
||||
[(pA, - F*q(t)/sqrt(q(t)**2)*N.x), (pB, F*q(t)/sqrt(q(t)**2)*N.x)]
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
force : Expr
|
||||
Magnitude of the force acting along the length of the pathway. As
|
||||
per the sign conventions for the pathway length, pathway extension
|
||||
velocity, and pair of point forces, if this ``Expr`` is positive
|
||||
then the force will act to push the pair of points away from one
|
||||
another (it is expansile).
|
||||
|
||||
"""
|
||||
relative_position = _point_pair_relative_position(*self.attachments)
|
||||
loads = [
|
||||
Force(self.attachments[0], -force*relative_position/self.length),
|
||||
Force(self.attachments[-1], force*relative_position/self.length),
|
||||
]
|
||||
return loads
|
||||
|
||||
|
||||
class ObstacleSetPathway(PathwayBase):
|
||||
"""Obstacle-set pathway between a set of attachment points.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
An obstacle-set pathway forms a series of straight-line segment between
|
||||
pairs of consecutive points in a set of points. It is similar to multiple
|
||||
linear pathways joined end-to-end. It will not interact with any other
|
||||
objects in the system, i.e. an ``ObstacleSetPathway`` will intersect other
|
||||
objects to ensure that the path between its pairs of points (its
|
||||
attachments) is the shortest possible.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
To construct an obstacle-set pathway, three or more points are required to
|
||||
be passed to the ``attachments`` parameter as a ``tuple``.
|
||||
|
||||
>>> from sympy.physics.mechanics import ObstacleSetPathway, Point
|
||||
>>> pA, pB, pC, pD = Point('pA'), Point('pB'), Point('pC'), Point('pD')
|
||||
>>> obstacle_set_pathway = ObstacleSetPathway(pA, pB, pC, pD)
|
||||
>>> obstacle_set_pathway
|
||||
ObstacleSetPathway(pA, pB, pC, pD)
|
||||
|
||||
The pathway created above isn't very interesting without the positions and
|
||||
velocities of its attachment points being described. Without this its not
|
||||
possible to describe how the pathway moves, i.e. its length or its
|
||||
extension velocity.
|
||||
|
||||
>>> from sympy import cos, sin
|
||||
>>> from sympy.physics.mechanics import ReferenceFrame
|
||||
>>> from sympy.physics.vector import dynamicsymbols
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> q = dynamicsymbols('q')
|
||||
>>> pO = Point('pO')
|
||||
>>> pA.set_pos(pO, N.y)
|
||||
>>> pB.set_pos(pO, -N.x)
|
||||
>>> pC.set_pos(pA, cos(q) * N.x - (sin(q) + 1) * N.y)
|
||||
>>> pD.set_pos(pA, sin(q) * N.x + (cos(q) - 1) * N.y)
|
||||
>>> pB.pos_from(pA)
|
||||
- N.x - N.y
|
||||
>>> pC.pos_from(pA)
|
||||
cos(q(t))*N.x + (-sin(q(t)) - 1)*N.y
|
||||
>>> pD.pos_from(pA)
|
||||
sin(q(t))*N.x + (cos(q(t)) - 1)*N.y
|
||||
|
||||
A pathway's length can be accessed via its ``length`` attribute.
|
||||
|
||||
>>> obstacle_set_pathway.length.simplify()
|
||||
sqrt(2)*(sqrt(cos(q(t)) + 1) + 2)
|
||||
|
||||
A pathway's extension velocity can be accessed similarly via its
|
||||
``extension_velocity`` attribute.
|
||||
|
||||
>>> obstacle_set_pathway.extension_velocity.simplify()
|
||||
-sqrt(2)*sin(q(t))*Derivative(q(t), t)/(2*sqrt(cos(q(t)) + 1))
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
attachments : tuple[Point, ...]
|
||||
The set of ``Point`` objects that define the segmented obstacle-set
|
||||
pathway.
|
||||
|
||||
"""
|
||||
|
||||
def __init__(self, *attachments):
|
||||
"""Initializer for ``ObstacleSetPathway``.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
attachments : tuple[Point, ...]
|
||||
The set of ``Point`` objects that define the segmented obstacle-set
|
||||
pathway.
|
||||
|
||||
"""
|
||||
super().__init__(*attachments)
|
||||
|
||||
@property
|
||||
def attachments(self):
|
||||
"""The set of points defining a pathway's segmented path."""
|
||||
return self._attachments
|
||||
|
||||
@attachments.setter
|
||||
def attachments(self, attachments):
|
||||
if hasattr(self, '_attachments'):
|
||||
msg = (
|
||||
f'Can\'t set attribute `attachments` to {repr(attachments)} '
|
||||
f'as it is immutable.'
|
||||
)
|
||||
raise AttributeError(msg)
|
||||
if len(attachments) <= 2:
|
||||
msg = (
|
||||
f'Value {repr(attachments)} passed to `attachments` was an '
|
||||
f'iterable of length {len(attachments)}, must be an iterable '
|
||||
f'of length 3 or greater.'
|
||||
)
|
||||
raise ValueError(msg)
|
||||
for i, point in enumerate(attachments):
|
||||
if not isinstance(point, Point):
|
||||
msg = (
|
||||
f'Value {repr(point)} passed to `attachments` at index '
|
||||
f'{i} was of type {type(point)}, must be {Point}.'
|
||||
)
|
||||
raise TypeError(msg)
|
||||
self._attachments = tuple(attachments)
|
||||
|
||||
@property
|
||||
def length(self):
|
||||
"""Exact analytical expression for the pathway's length."""
|
||||
length = S.Zero
|
||||
attachment_pairs = zip(self.attachments[:-1], self.attachments[1:])
|
||||
for attachment_pair in attachment_pairs:
|
||||
length += _point_pair_length(*attachment_pair)
|
||||
return length
|
||||
|
||||
@property
|
||||
def extension_velocity(self):
|
||||
"""Exact analytical expression for the pathway's extension velocity."""
|
||||
extension_velocity = S.Zero
|
||||
attachment_pairs = zip(self.attachments[:-1], self.attachments[1:])
|
||||
for attachment_pair in attachment_pairs:
|
||||
extension_velocity += _point_pair_extension_velocity(*attachment_pair)
|
||||
return extension_velocity
|
||||
|
||||
def to_loads(self, force):
|
||||
"""Loads required by the equations of motion method classes.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
``KanesMethod`` requires a list of ``Point``-``Vector`` tuples to be
|
||||
passed to the ``loads`` parameters of its ``kanes_equations`` method
|
||||
when constructing the equations of motion. This method acts as a
|
||||
utility to produce the correctly-structred pairs of points and vectors
|
||||
required so that these can be easily concatenated with other items in
|
||||
the list of loads and passed to ``KanesMethod.kanes_equations``. These
|
||||
loads are also in the correct form to also be passed to the other
|
||||
equations of motion method classes, e.g. ``LagrangesMethod``.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
The below example shows how to generate the loads produced in an
|
||||
actuator that follows an obstacle-set pathway between four points and
|
||||
produces an expansile force ``F``. First, create a pair of reference
|
||||
frames, ``A`` and ``B``, in which the four points ``pA``, ``pB``,
|
||||
``pC``, and ``pD`` will be located. The first two points in frame ``A``
|
||||
and the second two in frame ``B``. Frame ``B`` will also be oriented
|
||||
such that it relates to ``A`` via a rotation of ``q`` about an axis
|
||||
``N.z`` in a global frame (``N.z``, ``A.z``, and ``B.z`` are parallel).
|
||||
|
||||
>>> from sympy.physics.mechanics import (ObstacleSetPathway, Point,
|
||||
... ReferenceFrame)
|
||||
>>> from sympy.physics.vector import dynamicsymbols
|
||||
>>> q = dynamicsymbols('q')
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> A = N.orientnew('A', 'axis', (0, N.x))
|
||||
>>> B = A.orientnew('B', 'axis', (q, N.z))
|
||||
>>> pO = Point('pO')
|
||||
>>> pA, pB, pC, pD = Point('pA'), Point('pB'), Point('pC'), Point('pD')
|
||||
>>> pA.set_pos(pO, A.x)
|
||||
>>> pB.set_pos(pO, -A.y)
|
||||
>>> pC.set_pos(pO, B.y)
|
||||
>>> pD.set_pos(pO, B.x)
|
||||
>>> obstacle_set_pathway = ObstacleSetPathway(pA, pB, pC, pD)
|
||||
|
||||
Now create a symbol ``F`` to describe the magnitude of the (expansile)
|
||||
force that will be produced along the pathway. The list of loads that
|
||||
``KanesMethod`` requires can be produced by calling the pathway's
|
||||
``to_loads`` method with ``F`` passed as the only argument.
|
||||
|
||||
>>> from sympy import Symbol
|
||||
>>> F = Symbol('F')
|
||||
>>> obstacle_set_pathway.to_loads(F)
|
||||
[(pA, sqrt(2)*F/2*A.x + sqrt(2)*F/2*A.y),
|
||||
(pB, - sqrt(2)*F/2*A.x - sqrt(2)*F/2*A.y),
|
||||
(pB, - F/sqrt(2*cos(q(t)) + 2)*A.y - F/sqrt(2*cos(q(t)) + 2)*B.y),
|
||||
(pC, F/sqrt(2*cos(q(t)) + 2)*A.y + F/sqrt(2*cos(q(t)) + 2)*B.y),
|
||||
(pC, - sqrt(2)*F/2*B.x + sqrt(2)*F/2*B.y),
|
||||
(pD, sqrt(2)*F/2*B.x - sqrt(2)*F/2*B.y)]
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
force : Expr
|
||||
The force acting along the length of the pathway. It is assumed
|
||||
that this ``Expr`` represents an expansile force.
|
||||
|
||||
"""
|
||||
loads = []
|
||||
attachment_pairs = zip(self.attachments[:-1], self.attachments[1:])
|
||||
for attachment_pair in attachment_pairs:
|
||||
relative_position = _point_pair_relative_position(*attachment_pair)
|
||||
length = _point_pair_length(*attachment_pair)
|
||||
loads.extend([
|
||||
Force(attachment_pair[0], -force*relative_position/length),
|
||||
Force(attachment_pair[1], force*relative_position/length),
|
||||
])
|
||||
return loads
|
||||
|
||||
|
||||
class WrappingPathway(PathwayBase):
|
||||
"""Pathway that wraps a geometry object.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
A wrapping pathway interacts with a geometry object and forms a path that
|
||||
wraps smoothly along its surface. The wrapping pathway along the geometry
|
||||
object will be the geodesic that the geometry object defines based on the
|
||||
two points. It will not interact with any other objects in the system, i.e.
|
||||
a ``WrappingPathway`` will intersect other objects to ensure that the path
|
||||
between its two ends (its attachments) is the shortest possible.
|
||||
|
||||
To explain the sign conventions used for pathway length, extension
|
||||
velocity, and direction of applied forces, we can ignore the geometry with
|
||||
which the wrapping pathway interacts. A wrapping pathway is made up of two
|
||||
points that can move relative to each other, and a pair of equal and
|
||||
opposite forces acting on the points. If the positive time-varying
|
||||
Euclidean distance between the two points is defined, then the "extension
|
||||
velocity" is the time derivative of this distance. The extension velocity
|
||||
is positive when the two points are moving away from each other and
|
||||
negative when moving closer to each other. The direction for the force
|
||||
acting on either point is determined by constructing a unit vector directed
|
||||
from the other point to this point. This establishes a sign convention such
|
||||
that a positive force magnitude tends to push the points apart. The
|
||||
following diagram shows the positive force sense and the distance between
|
||||
the points::
|
||||
|
||||
P Q
|
||||
o<--- F --->o
|
||||
| |
|
||||
|<--l(t)--->|
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
>>> from sympy.physics.mechanics import WrappingPathway
|
||||
|
||||
To construct a wrapping pathway, like other pathways, a pair of points must
|
||||
be passed, followed by an instance of a wrapping geometry class as a
|
||||
keyword argument. We'll use a cylinder with radius ``r`` and its axis
|
||||
parallel to ``N.x`` passing through a point ``pO``.
|
||||
|
||||
>>> from sympy import symbols
|
||||
>>> from sympy.physics.mechanics import Point, ReferenceFrame, WrappingCylinder
|
||||
>>> r = symbols('r')
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> pA, pB, pO = Point('pA'), Point('pB'), Point('pO')
|
||||
>>> cylinder = WrappingCylinder(r, pO, N.x)
|
||||
>>> wrapping_pathway = WrappingPathway(pA, pB, cylinder)
|
||||
>>> wrapping_pathway
|
||||
WrappingPathway(pA, pB, geometry=WrappingCylinder(radius=r, point=pO,
|
||||
axis=N.x))
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
attachment_1 : Point
|
||||
First of the pair of ``Point`` objects between which the wrapping
|
||||
pathway spans.
|
||||
attachment_2 : Point
|
||||
Second of the pair of ``Point`` objects between which the wrapping
|
||||
pathway spans.
|
||||
geometry : WrappingGeometryBase
|
||||
Geometry about which the pathway wraps.
|
||||
|
||||
"""
|
||||
|
||||
def __init__(self, attachment_1, attachment_2, geometry):
|
||||
"""Initializer for ``WrappingPathway``.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
attachment_1 : Point
|
||||
First of the pair of ``Point`` objects between which the wrapping
|
||||
pathway spans.
|
||||
attachment_2 : Point
|
||||
Second of the pair of ``Point`` objects between which the wrapping
|
||||
pathway spans.
|
||||
geometry : WrappingGeometryBase
|
||||
Geometry about which the pathway wraps.
|
||||
The geometry about which the pathway wraps.
|
||||
|
||||
"""
|
||||
super().__init__(attachment_1, attachment_2)
|
||||
self.geometry = geometry
|
||||
|
||||
@property
|
||||
def geometry(self):
|
||||
"""Geometry around which the pathway wraps."""
|
||||
return self._geometry
|
||||
|
||||
@geometry.setter
|
||||
def geometry(self, geometry):
|
||||
if hasattr(self, '_geometry'):
|
||||
msg = (
|
||||
f'Can\'t set attribute `geometry` to {repr(geometry)} as it '
|
||||
f'is immutable.'
|
||||
)
|
||||
raise AttributeError(msg)
|
||||
if not isinstance(geometry, WrappingGeometryBase):
|
||||
msg = (
|
||||
f'Value {repr(geometry)} passed to `geometry` was of type '
|
||||
f'{type(geometry)}, must be {WrappingGeometryBase}.'
|
||||
)
|
||||
raise TypeError(msg)
|
||||
self._geometry = geometry
|
||||
|
||||
@property
|
||||
def length(self):
|
||||
"""Exact analytical expression for the pathway's length."""
|
||||
return self.geometry.geodesic_length(*self.attachments)
|
||||
|
||||
@property
|
||||
def extension_velocity(self):
|
||||
"""Exact analytical expression for the pathway's extension velocity."""
|
||||
return self.length.diff(dynamicsymbols._t)
|
||||
|
||||
def to_loads(self, force):
|
||||
"""Loads required by the equations of motion method classes.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
``KanesMethod`` requires a list of ``Point``-``Vector`` tuples to be
|
||||
passed to the ``loads`` parameters of its ``kanes_equations`` method
|
||||
when constructing the equations of motion. This method acts as a
|
||||
utility to produce the correctly-structred pairs of points and vectors
|
||||
required so that these can be easily concatenated with other items in
|
||||
the list of loads and passed to ``KanesMethod.kanes_equations``. These
|
||||
loads are also in the correct form to also be passed to the other
|
||||
equations of motion method classes, e.g. ``LagrangesMethod``.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
The below example shows how to generate the loads produced in an
|
||||
actuator that produces an expansile force ``F`` while wrapping around a
|
||||
cylinder. First, create a cylinder with radius ``r`` and an axis
|
||||
parallel to the ``N.z`` direction of the global frame ``N`` that also
|
||||
passes through a point ``pO``.
|
||||
|
||||
>>> from sympy import symbols
|
||||
>>> from sympy.physics.mechanics import (Point, ReferenceFrame,
|
||||
... WrappingCylinder)
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> r = symbols('r', positive=True)
|
||||
>>> pO = Point('pO')
|
||||
>>> cylinder = WrappingCylinder(r, pO, N.z)
|
||||
|
||||
Create the pathway of the actuator using the ``WrappingPathway`` class,
|
||||
defined to span between two points ``pA`` and ``pB``. Both points lie
|
||||
on the surface of the cylinder and the location of ``pB`` is defined
|
||||
relative to ``pA`` by the dynamics symbol ``q``.
|
||||
|
||||
>>> from sympy import cos, sin
|
||||
>>> from sympy.physics.mechanics import WrappingPathway, dynamicsymbols
|
||||
>>> q = dynamicsymbols('q')
|
||||
>>> pA = Point('pA')
|
||||
>>> pB = Point('pB')
|
||||
>>> pA.set_pos(pO, r*N.x)
|
||||
>>> pB.set_pos(pO, r*(cos(q)*N.x + sin(q)*N.y))
|
||||
>>> pB.pos_from(pA)
|
||||
(r*cos(q(t)) - r)*N.x + r*sin(q(t))*N.y
|
||||
>>> pathway = WrappingPathway(pA, pB, cylinder)
|
||||
|
||||
Now create a symbol ``F`` to describe the magnitude of the (expansile)
|
||||
force that will be produced along the pathway. The list of loads that
|
||||
``KanesMethod`` requires can be produced by calling the pathway's
|
||||
``to_loads`` method with ``F`` passed as the only argument.
|
||||
|
||||
>>> F = symbols('F')
|
||||
>>> loads = pathway.to_loads(F)
|
||||
>>> [load.__class__(load.location, load.vector.simplify()) for load in loads]
|
||||
[(pA, F*N.y), (pB, F*sin(q(t))*N.x - F*cos(q(t))*N.y),
|
||||
(pO, - F*sin(q(t))*N.x + F*(cos(q(t)) - 1)*N.y)]
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
force : Expr
|
||||
Magnitude of the force acting along the length of the pathway. It
|
||||
is assumed that this ``Expr`` represents an expansile force.
|
||||
|
||||
"""
|
||||
pA, pB = self.attachments
|
||||
pO = self.geometry.point
|
||||
pA_force, pB_force = self.geometry.geodesic_end_vectors(pA, pB)
|
||||
pO_force = -(pA_force + pB_force)
|
||||
|
||||
loads = [
|
||||
Force(pA, force * pA_force),
|
||||
Force(pB, force * pB_force),
|
||||
Force(pO, force * pO_force),
|
||||
]
|
||||
return loads
|
||||
|
||||
def __repr__(self):
|
||||
"""Representation of a ``WrappingPathway``."""
|
||||
attachments = ', '.join(str(a) for a in self.attachments)
|
||||
return (
|
||||
f'{self.__class__.__name__}({attachments}, '
|
||||
f'geometry={self.geometry})'
|
||||
)
|
||||
|
||||
|
||||
def _point_pair_relative_position(point_1, point_2):
|
||||
"""The relative position between a pair of points."""
|
||||
return point_2.pos_from(point_1)
|
||||
|
||||
|
||||
def _point_pair_length(point_1, point_2):
|
||||
"""The length of the direct linear path between two points."""
|
||||
return _point_pair_relative_position(point_1, point_2).magnitude()
|
||||
|
||||
|
||||
def _point_pair_extension_velocity(point_1, point_2):
|
||||
"""The extension velocity of the direct linear path between two points."""
|
||||
return _point_pair_length(point_1, point_2).diff(dynamicsymbols._t)
|
||||
+314
@@ -0,0 +1,314 @@
|
||||
from sympy import Symbol, S
|
||||
from sympy.physics.vector import ReferenceFrame, Dyadic, Point, dot
|
||||
from sympy.physics.mechanics.body_base import BodyBase
|
||||
from sympy.physics.mechanics.inertia import inertia_of_point_mass, Inertia
|
||||
from sympy.utilities.exceptions import sympy_deprecation_warning
|
||||
|
||||
__all__ = ['RigidBody']
|
||||
|
||||
|
||||
class RigidBody(BodyBase):
|
||||
"""An idealized rigid body.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
This is essentially a container which holds the various components which
|
||||
describe a rigid body: a name, mass, center of mass, reference frame, and
|
||||
inertia.
|
||||
|
||||
All of these need to be supplied on creation, but can be changed
|
||||
afterwards.
|
||||
|
||||
Attributes
|
||||
==========
|
||||
|
||||
name : string
|
||||
The body's name.
|
||||
masscenter : Point
|
||||
The point which represents the center of mass of the rigid body.
|
||||
frame : ReferenceFrame
|
||||
The ReferenceFrame which the rigid body is fixed in.
|
||||
mass : Sympifyable
|
||||
The body's mass.
|
||||
inertia : (Dyadic, Point)
|
||||
The body's inertia about a point; stored in a tuple as shown above.
|
||||
potential_energy : Sympifyable
|
||||
The potential energy of the RigidBody.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
>>> from sympy import Symbol
|
||||
>>> from sympy.physics.mechanics import ReferenceFrame, Point, RigidBody
|
||||
>>> from sympy.physics.mechanics import outer
|
||||
>>> m = Symbol('m')
|
||||
>>> A = ReferenceFrame('A')
|
||||
>>> P = Point('P')
|
||||
>>> I = outer (A.x, A.x)
|
||||
>>> inertia_tuple = (I, P)
|
||||
>>> B = RigidBody('B', P, A, m, inertia_tuple)
|
||||
>>> # Or you could change them afterwards
|
||||
>>> m2 = Symbol('m2')
|
||||
>>> B.mass = m2
|
||||
|
||||
"""
|
||||
|
||||
def __init__(self, name, masscenter=None, frame=None, mass=None,
|
||||
inertia=None):
|
||||
super().__init__(name, masscenter, mass)
|
||||
if frame is None:
|
||||
frame = ReferenceFrame(f'{name}_frame')
|
||||
self.frame = frame
|
||||
if inertia is None:
|
||||
ixx = Symbol(f'{name}_ixx')
|
||||
iyy = Symbol(f'{name}_iyy')
|
||||
izz = Symbol(f'{name}_izz')
|
||||
izx = Symbol(f'{name}_izx')
|
||||
ixy = Symbol(f'{name}_ixy')
|
||||
iyz = Symbol(f'{name}_iyz')
|
||||
inertia = Inertia.from_inertia_scalars(self.masscenter, self.frame,
|
||||
ixx, iyy, izz, ixy, iyz, izx)
|
||||
self.inertia = inertia
|
||||
|
||||
def __repr__(self):
|
||||
return (f'{self.__class__.__name__}({repr(self.name)}, masscenter='
|
||||
f'{repr(self.masscenter)}, frame={repr(self.frame)}, mass='
|
||||
f'{repr(self.mass)}, inertia={repr(self.inertia)})')
|
||||
|
||||
@property
|
||||
def frame(self):
|
||||
"""The ReferenceFrame fixed to the body."""
|
||||
return self._frame
|
||||
|
||||
@frame.setter
|
||||
def frame(self, F):
|
||||
if not isinstance(F, ReferenceFrame):
|
||||
raise TypeError("RigidBody frame must be a ReferenceFrame object.")
|
||||
self._frame = F
|
||||
|
||||
@property
|
||||
def x(self):
|
||||
"""The basis Vector for the body, in the x direction. """
|
||||
return self.frame.x
|
||||
|
||||
@property
|
||||
def y(self):
|
||||
"""The basis Vector for the body, in the y direction. """
|
||||
return self.frame.y
|
||||
|
||||
@property
|
||||
def z(self):
|
||||
"""The basis Vector for the body, in the z direction. """
|
||||
return self.frame.z
|
||||
|
||||
@property
|
||||
def inertia(self):
|
||||
"""The body's inertia about a point; stored as (Dyadic, Point)."""
|
||||
return self._inertia
|
||||
|
||||
@inertia.setter
|
||||
def inertia(self, I):
|
||||
# check if I is of the form (Dyadic, Point)
|
||||
if len(I) != 2 or not isinstance(I[0], Dyadic) or not isinstance(I[1], Point):
|
||||
raise TypeError("RigidBody inertia must be a tuple of the form (Dyadic, Point).")
|
||||
|
||||
self._inertia = Inertia(I[0], I[1])
|
||||
# have I S/O, want I S/S*
|
||||
# I S/O = I S/S* + I S*/O; I S/S* = I S/O - I S*/O
|
||||
# I_S/S* = I_S/O - I_S*/O
|
||||
I_Ss_O = inertia_of_point_mass(self.mass,
|
||||
self.masscenter.pos_from(I[1]),
|
||||
self.frame)
|
||||
self._central_inertia = I[0] - I_Ss_O
|
||||
|
||||
@property
|
||||
def central_inertia(self):
|
||||
"""The body's central inertia dyadic."""
|
||||
return self._central_inertia
|
||||
|
||||
@central_inertia.setter
|
||||
def central_inertia(self, I):
|
||||
if not isinstance(I, Dyadic):
|
||||
raise TypeError("RigidBody inertia must be a Dyadic object.")
|
||||
self.inertia = Inertia(I, self.masscenter)
|
||||
|
||||
def linear_momentum(self, frame):
|
||||
""" Linear momentum of the rigid body.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
The linear momentum L, of a rigid body B, with respect to frame N is
|
||||
given by:
|
||||
|
||||
``L = m * v``
|
||||
|
||||
where m is the mass of the rigid body, and v is the velocity of the mass
|
||||
center of B in the frame N.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
frame : ReferenceFrame
|
||||
The frame in which linear momentum is desired.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
>>> from sympy.physics.mechanics import Point, ReferenceFrame, outer
|
||||
>>> from sympy.physics.mechanics import RigidBody, dynamicsymbols
|
||||
>>> from sympy.physics.vector import init_vprinting
|
||||
>>> init_vprinting(pretty_print=False)
|
||||
>>> m, v = dynamicsymbols('m v')
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> P = Point('P')
|
||||
>>> P.set_vel(N, v * N.x)
|
||||
>>> I = outer (N.x, N.x)
|
||||
>>> Inertia_tuple = (I, P)
|
||||
>>> B = RigidBody('B', P, N, m, Inertia_tuple)
|
||||
>>> B.linear_momentum(N)
|
||||
m*v*N.x
|
||||
|
||||
"""
|
||||
|
||||
return self.mass * self.masscenter.vel(frame)
|
||||
|
||||
def angular_momentum(self, point, frame):
|
||||
"""Returns the angular momentum of the rigid body about a point in the
|
||||
given frame.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
The angular momentum H of a rigid body B about some point O in a frame N
|
||||
is given by:
|
||||
|
||||
``H = dot(I, w) + cross(r, m * v)``
|
||||
|
||||
where I and m are the central inertia dyadic and mass of rigid body B, w
|
||||
is the angular velocity of body B in the frame N, r is the position
|
||||
vector from point O to the mass center of B, and v is the velocity of
|
||||
the mass center in the frame N.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
point : Point
|
||||
The point about which angular momentum is desired.
|
||||
frame : ReferenceFrame
|
||||
The frame in which angular momentum is desired.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
>>> from sympy.physics.mechanics import Point, ReferenceFrame, outer
|
||||
>>> from sympy.physics.mechanics import RigidBody, dynamicsymbols
|
||||
>>> from sympy.physics.vector import init_vprinting
|
||||
>>> init_vprinting(pretty_print=False)
|
||||
>>> m, v, r, omega = dynamicsymbols('m v r omega')
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> b = ReferenceFrame('b')
|
||||
>>> b.set_ang_vel(N, omega * b.x)
|
||||
>>> P = Point('P')
|
||||
>>> P.set_vel(N, 1 * N.x)
|
||||
>>> I = outer(b.x, b.x)
|
||||
>>> B = RigidBody('B', P, b, m, (I, P))
|
||||
>>> B.angular_momentum(P, N)
|
||||
omega*b.x
|
||||
|
||||
"""
|
||||
I = self.central_inertia
|
||||
w = self.frame.ang_vel_in(frame)
|
||||
m = self.mass
|
||||
r = self.masscenter.pos_from(point)
|
||||
v = self.masscenter.vel(frame)
|
||||
|
||||
return I.dot(w) + r.cross(m * v)
|
||||
|
||||
def kinetic_energy(self, frame):
|
||||
"""Kinetic energy of the rigid body.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
The kinetic energy, T, of a rigid body, B, is given by:
|
||||
|
||||
``T = 1/2 * (dot(dot(I, w), w) + dot(m * v, v))``
|
||||
|
||||
where I and m are the central inertia dyadic and mass of rigid body B
|
||||
respectively, w is the body's angular velocity, and v is the velocity of
|
||||
the body's mass center in the supplied ReferenceFrame.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
frame : ReferenceFrame
|
||||
The RigidBody's angular velocity and the velocity of it's mass
|
||||
center are typically defined with respect to an inertial frame but
|
||||
any relevant frame in which the velocities are known can be
|
||||
supplied.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
>>> from sympy.physics.mechanics import Point, ReferenceFrame, outer
|
||||
>>> from sympy.physics.mechanics import RigidBody
|
||||
>>> from sympy import symbols
|
||||
>>> m, v, r, omega = symbols('m v r omega')
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> b = ReferenceFrame('b')
|
||||
>>> b.set_ang_vel(N, omega * b.x)
|
||||
>>> P = Point('P')
|
||||
>>> P.set_vel(N, v * N.x)
|
||||
>>> I = outer (b.x, b.x)
|
||||
>>> inertia_tuple = (I, P)
|
||||
>>> B = RigidBody('B', P, b, m, inertia_tuple)
|
||||
>>> B.kinetic_energy(N)
|
||||
m*v**2/2 + omega**2/2
|
||||
|
||||
"""
|
||||
|
||||
rotational_KE = S.Half * dot(
|
||||
self.frame.ang_vel_in(frame),
|
||||
dot(self.central_inertia, self.frame.ang_vel_in(frame)))
|
||||
translational_KE = S.Half * self.mass * dot(self.masscenter.vel(frame),
|
||||
self.masscenter.vel(frame))
|
||||
return rotational_KE + translational_KE
|
||||
|
||||
def set_potential_energy(self, scalar):
|
||||
sympy_deprecation_warning(
|
||||
"""
|
||||
The sympy.physics.mechanics.RigidBody.set_potential_energy()
|
||||
method is deprecated. Instead use
|
||||
|
||||
B.potential_energy = scalar
|
||||
""",
|
||||
deprecated_since_version="1.5",
|
||||
active_deprecations_target="deprecated-set-potential-energy",
|
||||
)
|
||||
self.potential_energy = scalar
|
||||
|
||||
def parallel_axis(self, point, frame=None):
|
||||
"""Returns the inertia dyadic of the body with respect to another point.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
point : sympy.physics.vector.Point
|
||||
The point to express the inertia dyadic about.
|
||||
frame : sympy.physics.vector.ReferenceFrame
|
||||
The reference frame used to construct the dyadic.
|
||||
|
||||
Returns
|
||||
=======
|
||||
|
||||
inertia : sympy.physics.vector.Dyadic
|
||||
The inertia dyadic of the rigid body expressed about the provided
|
||||
point.
|
||||
|
||||
"""
|
||||
if frame is None:
|
||||
frame = self.frame
|
||||
return self.central_inertia + inertia_of_point_mass(
|
||||
self.mass, self.masscenter.pos_from(point), frame)
|
||||
+1553
File diff suppressed because it is too large
Load Diff
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
+1084
File diff suppressed because it is too large
Load Diff
+340
@@ -0,0 +1,340 @@
|
||||
from sympy import (Symbol, symbols, sin, cos, Matrix, zeros,
|
||||
simplify)
|
||||
from sympy.physics.vector import Point, ReferenceFrame, dynamicsymbols, Dyadic
|
||||
from sympy.physics.mechanics import inertia, Body
|
||||
from sympy.testing.pytest import raises, warns_deprecated_sympy
|
||||
|
||||
|
||||
def test_default():
|
||||
with warns_deprecated_sympy():
|
||||
body = Body('body')
|
||||
assert body.name == 'body'
|
||||
assert body.loads == []
|
||||
point = Point('body_masscenter')
|
||||
point.set_vel(body.frame, 0)
|
||||
com = body.masscenter
|
||||
frame = body.frame
|
||||
assert com.vel(frame) == point.vel(frame)
|
||||
assert body.mass == Symbol('body_mass')
|
||||
ixx, iyy, izz = symbols('body_ixx body_iyy body_izz')
|
||||
ixy, iyz, izx = symbols('body_ixy body_iyz body_izx')
|
||||
assert body.inertia == (inertia(body.frame, ixx, iyy, izz, ixy, iyz, izx),
|
||||
body.masscenter)
|
||||
|
||||
|
||||
def test_custom_rigid_body():
|
||||
# Body with RigidBody.
|
||||
rigidbody_masscenter = Point('rigidbody_masscenter')
|
||||
rigidbody_mass = Symbol('rigidbody_mass')
|
||||
rigidbody_frame = ReferenceFrame('rigidbody_frame')
|
||||
body_inertia = inertia(rigidbody_frame, 1, 0, 0)
|
||||
with warns_deprecated_sympy():
|
||||
rigid_body = Body('rigidbody_body', rigidbody_masscenter,
|
||||
rigidbody_mass, rigidbody_frame, body_inertia)
|
||||
com = rigid_body.masscenter
|
||||
frame = rigid_body.frame
|
||||
rigidbody_masscenter.set_vel(rigidbody_frame, 0)
|
||||
assert com.vel(frame) == rigidbody_masscenter.vel(frame)
|
||||
assert com.pos_from(com) == rigidbody_masscenter.pos_from(com)
|
||||
|
||||
assert rigid_body.mass == rigidbody_mass
|
||||
assert rigid_body.inertia == (body_inertia, rigidbody_masscenter)
|
||||
|
||||
assert rigid_body.is_rigidbody
|
||||
|
||||
assert hasattr(rigid_body, 'masscenter')
|
||||
assert hasattr(rigid_body, 'mass')
|
||||
assert hasattr(rigid_body, 'frame')
|
||||
assert hasattr(rigid_body, 'inertia')
|
||||
|
||||
|
||||
def test_particle_body():
|
||||
# Body with Particle
|
||||
particle_masscenter = Point('particle_masscenter')
|
||||
particle_mass = Symbol('particle_mass')
|
||||
particle_frame = ReferenceFrame('particle_frame')
|
||||
with warns_deprecated_sympy():
|
||||
particle_body = Body('particle_body', particle_masscenter,
|
||||
particle_mass, particle_frame)
|
||||
com = particle_body.masscenter
|
||||
frame = particle_body.frame
|
||||
particle_masscenter.set_vel(particle_frame, 0)
|
||||
assert com.vel(frame) == particle_masscenter.vel(frame)
|
||||
assert com.pos_from(com) == particle_masscenter.pos_from(com)
|
||||
|
||||
assert particle_body.mass == particle_mass
|
||||
assert not hasattr(particle_body, "_inertia")
|
||||
assert hasattr(particle_body, 'frame')
|
||||
assert hasattr(particle_body, 'masscenter')
|
||||
assert hasattr(particle_body, 'mass')
|
||||
assert particle_body.inertia == (Dyadic(0), particle_body.masscenter)
|
||||
assert particle_body.central_inertia == Dyadic(0)
|
||||
assert not particle_body.is_rigidbody
|
||||
|
||||
particle_body.central_inertia = inertia(particle_frame, 1, 1, 1)
|
||||
assert particle_body.central_inertia == inertia(particle_frame, 1, 1, 1)
|
||||
assert particle_body.is_rigidbody
|
||||
|
||||
with warns_deprecated_sympy():
|
||||
particle_body = Body('particle_body', mass=particle_mass)
|
||||
assert not particle_body.is_rigidbody
|
||||
point = particle_body.masscenter.locatenew('point', particle_body.x)
|
||||
point_inertia = particle_mass * inertia(particle_body.frame, 0, 1, 1)
|
||||
particle_body.inertia = (point_inertia, point)
|
||||
assert particle_body.inertia == (point_inertia, point)
|
||||
assert particle_body.central_inertia == Dyadic(0)
|
||||
assert particle_body.is_rigidbody
|
||||
|
||||
|
||||
def test_particle_body_add_force():
|
||||
# Body with Particle
|
||||
particle_masscenter = Point('particle_masscenter')
|
||||
particle_mass = Symbol('particle_mass')
|
||||
particle_frame = ReferenceFrame('particle_frame')
|
||||
with warns_deprecated_sympy():
|
||||
particle_body = Body('particle_body', particle_masscenter,
|
||||
particle_mass, particle_frame)
|
||||
|
||||
a = Symbol('a')
|
||||
force_vector = a * particle_body.frame.x
|
||||
particle_body.apply_force(force_vector, particle_body.masscenter)
|
||||
assert len(particle_body.loads) == 1
|
||||
point = particle_body.masscenter.locatenew(
|
||||
particle_body._name + '_point0', 0)
|
||||
point.set_vel(particle_body.frame, 0)
|
||||
force_point = particle_body.loads[0][0]
|
||||
|
||||
frame = particle_body.frame
|
||||
assert force_point.vel(frame) == point.vel(frame)
|
||||
assert force_point.pos_from(force_point) == point.pos_from(force_point)
|
||||
|
||||
assert particle_body.loads[0][1] == force_vector
|
||||
|
||||
|
||||
def test_body_add_force():
|
||||
# Body with RigidBody.
|
||||
rigidbody_masscenter = Point('rigidbody_masscenter')
|
||||
rigidbody_mass = Symbol('rigidbody_mass')
|
||||
rigidbody_frame = ReferenceFrame('rigidbody_frame')
|
||||
body_inertia = inertia(rigidbody_frame, 1, 0, 0)
|
||||
with warns_deprecated_sympy():
|
||||
rigid_body = Body('rigidbody_body', rigidbody_masscenter,
|
||||
rigidbody_mass, rigidbody_frame, body_inertia)
|
||||
|
||||
l = Symbol('l')
|
||||
Fa = Symbol('Fa')
|
||||
point = rigid_body.masscenter.locatenew(
|
||||
'rigidbody_body_point0',
|
||||
l * rigid_body.frame.x)
|
||||
point.set_vel(rigid_body.frame, 0)
|
||||
force_vector = Fa * rigid_body.frame.z
|
||||
# apply_force with point
|
||||
rigid_body.apply_force(force_vector, point)
|
||||
assert len(rigid_body.loads) == 1
|
||||
force_point = rigid_body.loads[0][0]
|
||||
frame = rigid_body.frame
|
||||
assert force_point.vel(frame) == point.vel(frame)
|
||||
assert force_point.pos_from(force_point) == point.pos_from(force_point)
|
||||
assert rigid_body.loads[0][1] == force_vector
|
||||
# apply_force without point
|
||||
rigid_body.apply_force(force_vector)
|
||||
assert len(rigid_body.loads) == 2
|
||||
assert rigid_body.loads[1][1] == force_vector
|
||||
# passing something else than point
|
||||
raises(TypeError, lambda: rigid_body.apply_force(force_vector, 0))
|
||||
raises(TypeError, lambda: rigid_body.apply_force(0))
|
||||
|
||||
def test_body_add_torque():
|
||||
with warns_deprecated_sympy():
|
||||
body = Body('body')
|
||||
torque_vector = body.frame.x
|
||||
body.apply_torque(torque_vector)
|
||||
|
||||
assert len(body.loads) == 1
|
||||
assert body.loads[0] == (body.frame, torque_vector)
|
||||
raises(TypeError, lambda: body.apply_torque(0))
|
||||
|
||||
def test_body_masscenter_vel():
|
||||
with warns_deprecated_sympy():
|
||||
A = Body('A')
|
||||
N = ReferenceFrame('N')
|
||||
with warns_deprecated_sympy():
|
||||
B = Body('B', frame=N)
|
||||
A.masscenter.set_vel(N, N.z)
|
||||
assert A.masscenter_vel(B) == N.z
|
||||
assert A.masscenter_vel(N) == N.z
|
||||
|
||||
def test_body_ang_vel():
|
||||
with warns_deprecated_sympy():
|
||||
A = Body('A')
|
||||
N = ReferenceFrame('N')
|
||||
with warns_deprecated_sympy():
|
||||
B = Body('B', frame=N)
|
||||
A.frame.set_ang_vel(N, N.y)
|
||||
assert A.ang_vel_in(B) == N.y
|
||||
assert B.ang_vel_in(A) == -N.y
|
||||
assert A.ang_vel_in(N) == N.y
|
||||
|
||||
def test_body_dcm():
|
||||
with warns_deprecated_sympy():
|
||||
A = Body('A')
|
||||
B = Body('B')
|
||||
A.frame.orient_axis(B.frame, B.frame.z, 10)
|
||||
assert A.dcm(B) == Matrix([[cos(10), sin(10), 0], [-sin(10), cos(10), 0], [0, 0, 1]])
|
||||
assert A.dcm(B.frame) == Matrix([[cos(10), sin(10), 0], [-sin(10), cos(10), 0], [0, 0, 1]])
|
||||
|
||||
def test_body_axis():
|
||||
N = ReferenceFrame('N')
|
||||
with warns_deprecated_sympy():
|
||||
B = Body('B', frame=N)
|
||||
assert B.x == N.x
|
||||
assert B.y == N.y
|
||||
assert B.z == N.z
|
||||
|
||||
def test_apply_force_multiple_one_point():
|
||||
a, b = symbols('a b')
|
||||
P = Point('P')
|
||||
with warns_deprecated_sympy():
|
||||
B = Body('B')
|
||||
f1 = a*B.x
|
||||
f2 = b*B.y
|
||||
B.apply_force(f1, P)
|
||||
assert B.loads == [(P, f1)]
|
||||
B.apply_force(f2, P)
|
||||
assert B.loads == [(P, f1+f2)]
|
||||
|
||||
def test_apply_force():
|
||||
f, g = symbols('f g')
|
||||
q, x, v1, v2 = dynamicsymbols('q x v1 v2')
|
||||
P1 = Point('P1')
|
||||
P2 = Point('P2')
|
||||
with warns_deprecated_sympy():
|
||||
B1 = Body('B1')
|
||||
B2 = Body('B2')
|
||||
N = ReferenceFrame('N')
|
||||
|
||||
P1.set_vel(B1.frame, v1*B1.x)
|
||||
P2.set_vel(B2.frame, v2*B2.x)
|
||||
force = f*q*N.z # time varying force
|
||||
|
||||
B1.apply_force(force, P1, B2, P2) #applying equal and opposite force on moving points
|
||||
assert B1.loads == [(P1, force)]
|
||||
assert B2.loads == [(P2, -force)]
|
||||
|
||||
g1 = B1.mass*g*N.y
|
||||
g2 = B2.mass*g*N.y
|
||||
|
||||
B1.apply_force(g1) #applying gravity on B1 masscenter
|
||||
B2.apply_force(g2) #applying gravity on B2 masscenter
|
||||
|
||||
assert B1.loads == [(P1,force), (B1.masscenter, g1)]
|
||||
assert B2.loads == [(P2, -force), (B2.masscenter, g2)]
|
||||
|
||||
force2 = x*N.x
|
||||
|
||||
B1.apply_force(force2, reaction_body=B2) #Applying time varying force on masscenter
|
||||
|
||||
assert B1.loads == [(P1, force), (B1.masscenter, force2+g1)]
|
||||
assert B2.loads == [(P2, -force), (B2.masscenter, -force2+g2)]
|
||||
|
||||
def test_apply_torque():
|
||||
t = symbols('t')
|
||||
q = dynamicsymbols('q')
|
||||
with warns_deprecated_sympy():
|
||||
B1 = Body('B1')
|
||||
B2 = Body('B2')
|
||||
N = ReferenceFrame('N')
|
||||
torque = t*q*N.x
|
||||
|
||||
B1.apply_torque(torque, B2) #Applying equal and opposite torque
|
||||
assert B1.loads == [(B1.frame, torque)]
|
||||
assert B2.loads == [(B2.frame, -torque)]
|
||||
|
||||
torque2 = t*N.y
|
||||
B1.apply_torque(torque2)
|
||||
assert B1.loads == [(B1.frame, torque+torque2)]
|
||||
|
||||
def test_clear_load():
|
||||
a = symbols('a')
|
||||
P = Point('P')
|
||||
with warns_deprecated_sympy():
|
||||
B = Body('B')
|
||||
force = a*B.z
|
||||
B.apply_force(force, P)
|
||||
assert B.loads == [(P, force)]
|
||||
B.clear_loads()
|
||||
assert B.loads == []
|
||||
|
||||
def test_remove_load():
|
||||
P1 = Point('P1')
|
||||
P2 = Point('P2')
|
||||
with warns_deprecated_sympy():
|
||||
B = Body('B')
|
||||
f1 = B.x
|
||||
f2 = B.y
|
||||
B.apply_force(f1, P1)
|
||||
B.apply_force(f2, P2)
|
||||
assert B.loads == [(P1, f1), (P2, f2)]
|
||||
B.remove_load(P2)
|
||||
assert B.loads == [(P1, f1)]
|
||||
B.apply_torque(f1.cross(f2))
|
||||
assert B.loads == [(P1, f1), (B.frame, f1.cross(f2))]
|
||||
B.remove_load()
|
||||
assert B.loads == [(P1, f1)]
|
||||
|
||||
def test_apply_loads_on_multi_degree_freedom_holonomic_system():
|
||||
"""Example based on: https://pydy.readthedocs.io/en/latest/examples/multidof-holonomic.html"""
|
||||
with warns_deprecated_sympy():
|
||||
W = Body('W') #Wall
|
||||
B = Body('B') #Block
|
||||
P = Body('P') #Pendulum
|
||||
b = Body('b') #bob
|
||||
q1, q2 = dynamicsymbols('q1 q2') #generalized coordinates
|
||||
k, c, g, kT = symbols('k c g kT') #constants
|
||||
F, T = dynamicsymbols('F T') #Specified forces
|
||||
|
||||
#Applying forces
|
||||
B.apply_force(F*W.x)
|
||||
W.apply_force(k*q1*W.x, reaction_body=B) #Spring force
|
||||
W.apply_force(c*q1.diff()*W.x, reaction_body=B) #dampner
|
||||
P.apply_force(P.mass*g*W.y)
|
||||
b.apply_force(b.mass*g*W.y)
|
||||
|
||||
#Applying torques
|
||||
P.apply_torque(kT*q2*W.z, reaction_body=b)
|
||||
P.apply_torque(T*W.z)
|
||||
|
||||
assert B.loads == [(B.masscenter, (F - k*q1 - c*q1.diff())*W.x)]
|
||||
assert P.loads == [(P.masscenter, P.mass*g*W.y), (P.frame, (T + kT*q2)*W.z)]
|
||||
assert b.loads == [(b.masscenter, b.mass*g*W.y), (b.frame, -kT*q2*W.z)]
|
||||
assert W.loads == [(W.masscenter, (c*q1.diff() + k*q1)*W.x)]
|
||||
|
||||
|
||||
def test_parallel_axis():
|
||||
N = ReferenceFrame('N')
|
||||
m, Ix, Iy, Iz, a, b = symbols('m, I_x, I_y, I_z, a, b')
|
||||
Io = inertia(N, Ix, Iy, Iz)
|
||||
# Test RigidBody
|
||||
o = Point('o')
|
||||
p = o.locatenew('p', a * N.x + b * N.y)
|
||||
with warns_deprecated_sympy():
|
||||
R = Body('R', masscenter=o, frame=N, mass=m, central_inertia=Io)
|
||||
Ip = R.parallel_axis(p)
|
||||
Ip_expected = inertia(N, Ix + m * b**2, Iy + m * a**2,
|
||||
Iz + m * (a**2 + b**2), ixy=-m * a * b)
|
||||
assert Ip == Ip_expected
|
||||
# Reference frame from which the parallel axis is viewed should not matter
|
||||
A = ReferenceFrame('A')
|
||||
A.orient_axis(N, N.z, 1)
|
||||
assert simplify(
|
||||
(R.parallel_axis(p, A) - Ip_expected).to_matrix(A)) == zeros(3, 3)
|
||||
# Test Particle
|
||||
o = Point('o')
|
||||
p = o.locatenew('p', a * N.x + b * N.y)
|
||||
with warns_deprecated_sympy():
|
||||
P = Body('P', masscenter=o, mass=m, frame=N)
|
||||
Ip = P.parallel_axis(p, N)
|
||||
Ip_expected = inertia(N, m * b ** 2, m * a ** 2, m * (a ** 2 + b ** 2),
|
||||
ixy=-m * a * b)
|
||||
assert not P.is_rigidbody
|
||||
assert Ip == Ip_expected
|
||||
+262
@@ -0,0 +1,262 @@
|
||||
from sympy import sin, cos, tan, pi, symbols, Matrix, S, Function
|
||||
from sympy.physics.mechanics import (Particle, Point, ReferenceFrame,
|
||||
RigidBody)
|
||||
from sympy.physics.mechanics import (angular_momentum, dynamicsymbols,
|
||||
kinetic_energy, linear_momentum,
|
||||
outer, potential_energy, msubs,
|
||||
find_dynamicsymbols, Lagrangian)
|
||||
|
||||
from sympy.physics.mechanics.functions import (
|
||||
center_of_mass, _validate_coordinates, _parse_linear_solver)
|
||||
from sympy.testing.pytest import raises, warns_deprecated_sympy
|
||||
|
||||
|
||||
q1, q2, q3, q4, q5 = symbols('q1 q2 q3 q4 q5')
|
||||
N = ReferenceFrame('N')
|
||||
A = N.orientnew('A', 'Axis', [q1, N.z])
|
||||
B = A.orientnew('B', 'Axis', [q2, A.x])
|
||||
C = B.orientnew('C', 'Axis', [q3, B.y])
|
||||
|
||||
|
||||
def test_linear_momentum():
|
||||
N = ReferenceFrame('N')
|
||||
Ac = Point('Ac')
|
||||
Ac.set_vel(N, 25 * N.y)
|
||||
I = outer(N.x, N.x)
|
||||
A = RigidBody('A', Ac, N, 20, (I, Ac))
|
||||
P = Point('P')
|
||||
Pa = Particle('Pa', P, 1)
|
||||
Pa.point.set_vel(N, 10 * N.x)
|
||||
raises(TypeError, lambda: linear_momentum(A, A, Pa))
|
||||
raises(TypeError, lambda: linear_momentum(N, N, Pa))
|
||||
assert linear_momentum(N, A, Pa) == 10 * N.x + 500 * N.y
|
||||
|
||||
|
||||
def test_angular_momentum_and_linear_momentum():
|
||||
"""A rod with length 2l, centroidal inertia I, and mass M along with a
|
||||
particle of mass m fixed to the end of the rod rotate with an angular rate
|
||||
of omega about point O which is fixed to the non-particle end of the rod.
|
||||
The rod's reference frame is A and the inertial frame is N."""
|
||||
m, M, l, I = symbols('m, M, l, I')
|
||||
omega = dynamicsymbols('omega')
|
||||
N = ReferenceFrame('N')
|
||||
a = ReferenceFrame('a')
|
||||
O = Point('O')
|
||||
Ac = O.locatenew('Ac', l * N.x)
|
||||
P = Ac.locatenew('P', l * N.x)
|
||||
O.set_vel(N, 0 * N.x)
|
||||
a.set_ang_vel(N, omega * N.z)
|
||||
Ac.v2pt_theory(O, N, a)
|
||||
P.v2pt_theory(O, N, a)
|
||||
Pa = Particle('Pa', P, m)
|
||||
A = RigidBody('A', Ac, a, M, (I * outer(N.z, N.z), Ac))
|
||||
expected = 2 * m * omega * l * N.y + M * l * omega * N.y
|
||||
assert linear_momentum(N, A, Pa) == expected
|
||||
raises(TypeError, lambda: angular_momentum(N, N, A, Pa))
|
||||
raises(TypeError, lambda: angular_momentum(O, O, A, Pa))
|
||||
raises(TypeError, lambda: angular_momentum(O, N, O, Pa))
|
||||
expected = (I + M * l**2 + 4 * m * l**2) * omega * N.z
|
||||
assert angular_momentum(O, N, A, Pa) == expected
|
||||
|
||||
|
||||
def test_kinetic_energy():
|
||||
m, M, l1 = symbols('m M l1')
|
||||
omega = dynamicsymbols('omega')
|
||||
N = ReferenceFrame('N')
|
||||
O = Point('O')
|
||||
O.set_vel(N, 0 * N.x)
|
||||
Ac = O.locatenew('Ac', l1 * N.x)
|
||||
P = Ac.locatenew('P', l1 * N.x)
|
||||
a = ReferenceFrame('a')
|
||||
a.set_ang_vel(N, omega * N.z)
|
||||
Ac.v2pt_theory(O, N, a)
|
||||
P.v2pt_theory(O, N, a)
|
||||
Pa = Particle('Pa', P, m)
|
||||
I = outer(N.z, N.z)
|
||||
A = RigidBody('A', Ac, a, M, (I, Ac))
|
||||
raises(TypeError, lambda: kinetic_energy(Pa, Pa, A))
|
||||
raises(TypeError, lambda: kinetic_energy(N, N, A))
|
||||
assert 0 == (kinetic_energy(N, Pa, A) - (M*l1**2*omega**2/2
|
||||
+ 2*l1**2*m*omega**2 + omega**2/2)).expand()
|
||||
|
||||
|
||||
def test_potential_energy():
|
||||
m, M, l1, g, h, H = symbols('m M l1 g h H')
|
||||
omega = dynamicsymbols('omega')
|
||||
N = ReferenceFrame('N')
|
||||
O = Point('O')
|
||||
O.set_vel(N, 0 * N.x)
|
||||
Ac = O.locatenew('Ac', l1 * N.x)
|
||||
P = Ac.locatenew('P', l1 * N.x)
|
||||
a = ReferenceFrame('a')
|
||||
a.set_ang_vel(N, omega * N.z)
|
||||
Ac.v2pt_theory(O, N, a)
|
||||
P.v2pt_theory(O, N, a)
|
||||
Pa = Particle('Pa', P, m)
|
||||
I = outer(N.z, N.z)
|
||||
A = RigidBody('A', Ac, a, M, (I, Ac))
|
||||
Pa.potential_energy = m * g * h
|
||||
A.potential_energy = M * g * H
|
||||
assert potential_energy(A, Pa) == m * g * h + M * g * H
|
||||
|
||||
|
||||
def test_Lagrangian():
|
||||
M, m, g, h = symbols('M m g h')
|
||||
N = ReferenceFrame('N')
|
||||
O = Point('O')
|
||||
O.set_vel(N, 0 * N.x)
|
||||
P = O.locatenew('P', 1 * N.x)
|
||||
P.set_vel(N, 10 * N.x)
|
||||
Pa = Particle('Pa', P, 1)
|
||||
Ac = O.locatenew('Ac', 2 * N.y)
|
||||
Ac.set_vel(N, 5 * N.y)
|
||||
a = ReferenceFrame('a')
|
||||
a.set_ang_vel(N, 10 * N.z)
|
||||
I = outer(N.z, N.z)
|
||||
A = RigidBody('A', Ac, a, 20, (I, Ac))
|
||||
Pa.potential_energy = m * g * h
|
||||
A.potential_energy = M * g * h
|
||||
raises(TypeError, lambda: Lagrangian(A, A, Pa))
|
||||
raises(TypeError, lambda: Lagrangian(N, N, Pa))
|
||||
|
||||
|
||||
def test_msubs():
|
||||
a, b = symbols('a, b')
|
||||
x, y, z = dynamicsymbols('x, y, z')
|
||||
# Test simple substitution
|
||||
expr = Matrix([[a*x + b, x*y.diff() + y],
|
||||
[x.diff().diff(), z + sin(z.diff())]])
|
||||
sol = Matrix([[a + b, y],
|
||||
[x.diff().diff(), 1]])
|
||||
sd = {x: 1, z: 1, z.diff(): 0, y.diff(): 0}
|
||||
assert msubs(expr, sd) == sol
|
||||
# Test smart substitution
|
||||
expr = cos(x + y)*tan(x + y) + b*x.diff()
|
||||
sd = {x: 0, y: pi/2, x.diff(): 1}
|
||||
assert msubs(expr, sd, smart=True) == b + 1
|
||||
N = ReferenceFrame('N')
|
||||
v = x*N.x + y*N.y
|
||||
d = x*(N.x|N.x) + y*(N.y|N.y)
|
||||
v_sol = 1*N.y
|
||||
d_sol = 1*(N.y|N.y)
|
||||
sd = {x: 0, y: 1}
|
||||
assert msubs(v, sd) == v_sol
|
||||
assert msubs(d, sd) == d_sol
|
||||
|
||||
|
||||
def test_find_dynamicsymbols():
|
||||
a, b = symbols('a, b')
|
||||
x, y, z = dynamicsymbols('x, y, z')
|
||||
expr = Matrix([[a*x + b, x*y.diff() + y],
|
||||
[x.diff().diff(), z + sin(z.diff())]])
|
||||
# Test finding all dynamicsymbols
|
||||
sol = {x, y.diff(), y, x.diff().diff(), z, z.diff()}
|
||||
assert find_dynamicsymbols(expr) == sol
|
||||
# Test finding all but those in sym_list
|
||||
exclude_list = [x, y, z]
|
||||
sol = {y.diff(), x.diff().diff(), z.diff()}
|
||||
assert find_dynamicsymbols(expr, exclude=exclude_list) == sol
|
||||
# Test finding all dynamicsymbols in a vector with a given reference frame
|
||||
d, e, f = dynamicsymbols('d, e, f')
|
||||
A = ReferenceFrame('A')
|
||||
v = d * A.x + e * A.y + f * A.z
|
||||
sol = {d, e, f}
|
||||
assert find_dynamicsymbols(v, reference_frame=A) == sol
|
||||
# Test if a ValueError is raised on supplying only a vector as input
|
||||
raises(ValueError, lambda: find_dynamicsymbols(v))
|
||||
|
||||
|
||||
# This function tests the center_of_mass() function
|
||||
# that was added in PR #14758 to compute the center of
|
||||
# mass of a system of bodies.
|
||||
def test_center_of_mass():
|
||||
a = ReferenceFrame('a')
|
||||
m = symbols('m', real=True)
|
||||
p1 = Particle('p1', Point('p1_pt'), S.One)
|
||||
p2 = Particle('p2', Point('p2_pt'), S(2))
|
||||
p3 = Particle('p3', Point('p3_pt'), S(3))
|
||||
p4 = Particle('p4', Point('p4_pt'), m)
|
||||
b_f = ReferenceFrame('b_f')
|
||||
b_cm = Point('b_cm')
|
||||
mb = symbols('mb')
|
||||
b = RigidBody('b', b_cm, b_f, mb, (outer(b_f.x, b_f.x), b_cm))
|
||||
p2.point.set_pos(p1.point, a.x)
|
||||
p3.point.set_pos(p1.point, a.x + a.y)
|
||||
p4.point.set_pos(p1.point, a.y)
|
||||
b.masscenter.set_pos(p1.point, a.y + a.z)
|
||||
point_o=Point('o')
|
||||
point_o.set_pos(p1.point, center_of_mass(p1.point, p1, p2, p3, p4, b))
|
||||
expr = 5/(m + mb + 6)*a.x + (m + mb + 3)/(m + mb + 6)*a.y + mb/(m + mb + 6)*a.z
|
||||
assert point_o.pos_from(p1.point)-expr == 0
|
||||
|
||||
|
||||
def test_validate_coordinates():
|
||||
q1, q2, q3, u1, u2, u3, ua1, ua2, ua3 = dynamicsymbols('q1:4 u1:4 ua1:4')
|
||||
s1, s2, s3 = symbols('s1:4')
|
||||
# Test normal
|
||||
_validate_coordinates([q1, q2, q3], [u1, u2, u3],
|
||||
u_auxiliary=[ua1, ua2, ua3])
|
||||
# Test not equal number of coordinates and speeds
|
||||
_validate_coordinates([q1, q2])
|
||||
_validate_coordinates([q1, q2], [u1])
|
||||
_validate_coordinates(speeds=[u1, u2])
|
||||
# Test duplicate
|
||||
_validate_coordinates([q1, q2, q2], [u1, u2, u3], check_duplicates=False)
|
||||
raises(ValueError, lambda: _validate_coordinates(
|
||||
[q1, q2, q2], [u1, u2, u3]))
|
||||
_validate_coordinates([q1, q2, q3], [u1, u2, u2], check_duplicates=False)
|
||||
raises(ValueError, lambda: _validate_coordinates(
|
||||
[q1, q2, q3], [u1, u2, u2], check_duplicates=True))
|
||||
raises(ValueError, lambda: _validate_coordinates(
|
||||
[q1, q2, q3], [q1, u2, u3], check_duplicates=True))
|
||||
_validate_coordinates([q1, q2, q3], [u1, u2, u3], check_duplicates=False,
|
||||
u_auxiliary=[u1, ua2, ua2])
|
||||
raises(ValueError, lambda: _validate_coordinates(
|
||||
[q1, q2, q3], [u1, u2, u3], u_auxiliary=[u1, ua2, ua3]))
|
||||
raises(ValueError, lambda: _validate_coordinates(
|
||||
[q1, q2, q3], [u1, u2, u3], u_auxiliary=[q1, ua2, ua3]))
|
||||
raises(ValueError, lambda: _validate_coordinates(
|
||||
[q1, q2, q3], [u1, u2, u3], u_auxiliary=[ua1, ua2, ua2]))
|
||||
# Test is_dynamicsymbols
|
||||
_validate_coordinates([q1 + q2, q3], is_dynamicsymbols=False)
|
||||
raises(ValueError, lambda: _validate_coordinates([q1 + q2, q3]))
|
||||
_validate_coordinates([s1, q1, q2], [0, u1, u2], is_dynamicsymbols=False)
|
||||
raises(ValueError, lambda: _validate_coordinates(
|
||||
[s1, q1, q2], [0, u1, u2], is_dynamicsymbols=True))
|
||||
_validate_coordinates([s1 + s2 + s3, q1], [0, u1], is_dynamicsymbols=False)
|
||||
raises(ValueError, lambda: _validate_coordinates(
|
||||
[s1 + s2 + s3, q1], [0, u1], is_dynamicsymbols=True))
|
||||
_validate_coordinates(u_auxiliary=[s1, ua1], is_dynamicsymbols=False)
|
||||
raises(ValueError, lambda: _validate_coordinates(u_auxiliary=[s1, ua1]))
|
||||
# Test normal function
|
||||
t = dynamicsymbols._t
|
||||
a = symbols('a')
|
||||
f1, f2 = symbols('f1:3', cls=Function)
|
||||
_validate_coordinates([f1(a), f2(a)], is_dynamicsymbols=False)
|
||||
raises(ValueError, lambda: _validate_coordinates([f1(a), f2(a)]))
|
||||
raises(ValueError, lambda: _validate_coordinates(speeds=[f1(a), f2(a)]))
|
||||
dynamicsymbols._t = a
|
||||
_validate_coordinates([f1(a), f2(a)])
|
||||
raises(ValueError, lambda: _validate_coordinates([f1(t), f2(t)]))
|
||||
dynamicsymbols._t = t
|
||||
|
||||
|
||||
def test_parse_linear_solver():
|
||||
A, b = Matrix(3, 3, symbols('a:9')), Matrix(3, 2, symbols('b:6'))
|
||||
assert _parse_linear_solver(Matrix.LUsolve) == Matrix.LUsolve # Test callable
|
||||
assert _parse_linear_solver('LU')(A, b) == Matrix.LUsolve(A, b)
|
||||
|
||||
|
||||
def test_deprecated_moved_functions():
|
||||
from sympy.physics.mechanics.functions import (
|
||||
inertia, inertia_of_point_mass, gravity)
|
||||
N = ReferenceFrame('N')
|
||||
with warns_deprecated_sympy():
|
||||
assert inertia(N, 0, 1, 0, 1) == (N.x | N.y) + (N.y | N.x) + (N.y | N.y)
|
||||
with warns_deprecated_sympy():
|
||||
assert inertia_of_point_mass(1, N.x + N.y, N) == (
|
||||
(N.x | N.x) + (N.y | N.y) + 2 * (N.z | N.z) -
|
||||
(N.x | N.y) - (N.y | N.x))
|
||||
p = Particle('P')
|
||||
with warns_deprecated_sympy():
|
||||
assert gravity(-2 * N.z, p) == [(p.masscenter, -2 * p.mass * N.z)]
|
||||
+71
@@ -0,0 +1,71 @@
|
||||
from sympy import symbols
|
||||
from sympy.testing.pytest import raises
|
||||
from sympy.physics.mechanics import (inertia, inertia_of_point_mass,
|
||||
Inertia, ReferenceFrame, Point)
|
||||
|
||||
|
||||
def test_inertia_dyadic():
|
||||
N = ReferenceFrame('N')
|
||||
ixx, iyy, izz = symbols('ixx iyy izz')
|
||||
ixy, iyz, izx = symbols('ixy iyz izx')
|
||||
assert inertia(N, ixx, iyy, izz) == (ixx * (N.x | N.x) + iyy *
|
||||
(N.y | N.y) + izz * (N.z | N.z))
|
||||
assert inertia(N, 0, 0, 0) == 0 * (N.x | N.x)
|
||||
raises(TypeError, lambda: inertia(0, 0, 0, 0))
|
||||
assert inertia(N, ixx, iyy, izz, ixy, iyz, izx) == (ixx * (N.x | N.x) +
|
||||
ixy * (N.x | N.y) + izx * (N.x | N.z) + ixy * (N.y | N.x) + iyy *
|
||||
(N.y | N.y) + iyz * (N.y | N.z) + izx * (N.z | N.x) + iyz * (N.z |
|
||||
N.y) + izz * (N.z | N.z))
|
||||
|
||||
|
||||
def test_inertia_of_point_mass():
|
||||
r, s, t, m = symbols('r s t m')
|
||||
N = ReferenceFrame('N')
|
||||
|
||||
px = r * N.x
|
||||
I = inertia_of_point_mass(m, px, N)
|
||||
assert I == m * r**2 * (N.y | N.y) + m * r**2 * (N.z | N.z)
|
||||
|
||||
py = s * N.y
|
||||
I = inertia_of_point_mass(m, py, N)
|
||||
assert I == m * s**2 * (N.x | N.x) + m * s**2 * (N.z | N.z)
|
||||
|
||||
pz = t * N.z
|
||||
I = inertia_of_point_mass(m, pz, N)
|
||||
assert I == m * t**2 * (N.x | N.x) + m * t**2 * (N.y | N.y)
|
||||
|
||||
p = px + py + pz
|
||||
I = inertia_of_point_mass(m, p, N)
|
||||
assert I == (m * (s**2 + t**2) * (N.x | N.x) -
|
||||
m * r * s * (N.x | N.y) -
|
||||
m * r * t * (N.x | N.z) -
|
||||
m * r * s * (N.y | N.x) +
|
||||
m * (r**2 + t**2) * (N.y | N.y) -
|
||||
m * s * t * (N.y | N.z) -
|
||||
m * r * t * (N.z | N.x) -
|
||||
m * s * t * (N.z | N.y) +
|
||||
m * (r**2 + s**2) * (N.z | N.z))
|
||||
|
||||
|
||||
def test_inertia_object():
|
||||
N = ReferenceFrame('N')
|
||||
O = Point('O')
|
||||
ixx, iyy, izz = symbols('ixx iyy izz')
|
||||
I_dyadic = ixx * (N.x | N.x) + iyy * (N.y | N.y) + izz * (N.z | N.z)
|
||||
I = Inertia(inertia(N, ixx, iyy, izz), O)
|
||||
assert isinstance(I, tuple)
|
||||
assert I.__repr__() == ('Inertia(dyadic=ixx*(N.x|N.x) + iyy*(N.y|N.y) + '
|
||||
'izz*(N.z|N.z), point=O)')
|
||||
assert I.dyadic == I_dyadic
|
||||
assert I.point == O
|
||||
assert I[0] == I_dyadic
|
||||
assert I[1] == O
|
||||
assert I == (I_dyadic, O) # Test tuple equal
|
||||
raises(TypeError, lambda: I != (O, I_dyadic)) # Incorrect tuple order
|
||||
assert I == Inertia(O, I_dyadic) # Parse changed argument order
|
||||
assert I == Inertia.from_inertia_scalars(O, N, ixx, iyy, izz)
|
||||
# Test invalid tuple operations
|
||||
raises(TypeError, lambda: I + (1, 2))
|
||||
raises(TypeError, lambda: (1, 2) + I)
|
||||
raises(TypeError, lambda: I * 2)
|
||||
raises(TypeError, lambda: 2 * I)
|
||||
+1240
File diff suppressed because it is too large
Load Diff
+249
@@ -0,0 +1,249 @@
|
||||
from sympy.core.function import expand
|
||||
from sympy.core.symbol import symbols
|
||||
from sympy.functions.elementary.trigonometric import (cos, sin)
|
||||
from sympy.matrices.dense import Matrix
|
||||
from sympy.simplify.trigsimp import trigsimp
|
||||
from sympy.physics.mechanics import (
|
||||
PinJoint, JointsMethod, RigidBody, Particle, Body, KanesMethod,
|
||||
PrismaticJoint, LagrangesMethod, inertia)
|
||||
from sympy.physics.vector import dynamicsymbols, ReferenceFrame
|
||||
from sympy.testing.pytest import raises, warns_deprecated_sympy
|
||||
from sympy import zeros
|
||||
from sympy.utilities.lambdify import lambdify
|
||||
from sympy.solvers.solvers import solve
|
||||
|
||||
|
||||
t = dynamicsymbols._t # type: ignore
|
||||
|
||||
|
||||
def test_jointsmethod():
|
||||
with warns_deprecated_sympy():
|
||||
P = Body('P')
|
||||
C = Body('C')
|
||||
Pin = PinJoint('P1', P, C)
|
||||
C_ixx, g = symbols('C_ixx g')
|
||||
q, u = dynamicsymbols('q_P1, u_P1')
|
||||
P.apply_force(g*P.y)
|
||||
with warns_deprecated_sympy():
|
||||
method = JointsMethod(P, Pin)
|
||||
assert method.frame == P.frame
|
||||
assert method.bodies == [C, P]
|
||||
assert method.loads == [(P.masscenter, g*P.frame.y)]
|
||||
assert method.q == Matrix([q])
|
||||
assert method.u == Matrix([u])
|
||||
assert method.kdes == Matrix([u - q.diff()])
|
||||
soln = method.form_eoms()
|
||||
assert soln == Matrix([[-C_ixx*u.diff()]])
|
||||
assert method.forcing_full == Matrix([[u], [0]])
|
||||
assert method.mass_matrix_full == Matrix([[1, 0], [0, C_ixx]])
|
||||
assert isinstance(method.method, KanesMethod)
|
||||
|
||||
|
||||
def test_rigid_body_particle_compatibility():
|
||||
l, m, g = symbols('l m g')
|
||||
C = RigidBody('C')
|
||||
b = Particle('b', mass=m)
|
||||
b_frame = ReferenceFrame('b_frame')
|
||||
q, u = dynamicsymbols('q u')
|
||||
P = PinJoint('P', C, b, coordinates=q, speeds=u, child_interframe=b_frame,
|
||||
child_point=-l * b_frame.x, joint_axis=C.z)
|
||||
with warns_deprecated_sympy():
|
||||
method = JointsMethod(C, P)
|
||||
method.loads.append((b.masscenter, m * g * C.x))
|
||||
method.form_eoms()
|
||||
rhs = method.rhs()
|
||||
assert rhs[1] == -g*sin(q)/l
|
||||
|
||||
|
||||
def test_jointmethod_duplicate_coordinates_speeds():
|
||||
with warns_deprecated_sympy():
|
||||
P = Body('P')
|
||||
C = Body('C')
|
||||
T = Body('T')
|
||||
q, u = dynamicsymbols('q u')
|
||||
P1 = PinJoint('P1', P, C, q)
|
||||
P2 = PrismaticJoint('P2', C, T, q)
|
||||
with warns_deprecated_sympy():
|
||||
raises(ValueError, lambda: JointsMethod(P, P1, P2))
|
||||
|
||||
P1 = PinJoint('P1', P, C, speeds=u)
|
||||
P2 = PrismaticJoint('P2', C, T, speeds=u)
|
||||
with warns_deprecated_sympy():
|
||||
raises(ValueError, lambda: JointsMethod(P, P1, P2))
|
||||
|
||||
P1 = PinJoint('P1', P, C, q, u)
|
||||
P2 = PrismaticJoint('P2', C, T, q, u)
|
||||
with warns_deprecated_sympy():
|
||||
raises(ValueError, lambda: JointsMethod(P, P1, P2))
|
||||
|
||||
def test_complete_simple_double_pendulum():
|
||||
q1, q2 = dynamicsymbols('q1 q2')
|
||||
u1, u2 = dynamicsymbols('u1 u2')
|
||||
m, l, g = symbols('m l g')
|
||||
with warns_deprecated_sympy():
|
||||
C = Body('C') # ceiling
|
||||
PartP = Body('P', mass=m)
|
||||
PartR = Body('R', mass=m)
|
||||
J1 = PinJoint('J1', C, PartP, speeds=u1, coordinates=q1,
|
||||
child_point=-l*PartP.x, joint_axis=C.z)
|
||||
J2 = PinJoint('J2', PartP, PartR, speeds=u2, coordinates=q2,
|
||||
child_point=-l*PartR.x, joint_axis=PartP.z)
|
||||
|
||||
PartP.apply_force(m*g*C.x)
|
||||
PartR.apply_force(m*g*C.x)
|
||||
|
||||
with warns_deprecated_sympy():
|
||||
method = JointsMethod(C, J1, J2)
|
||||
method.form_eoms()
|
||||
|
||||
assert expand(method.mass_matrix_full) == Matrix([[1, 0, 0, 0],
|
||||
[0, 1, 0, 0],
|
||||
[0, 0, 2*l**2*m*cos(q2) + 3*l**2*m, l**2*m*cos(q2) + l**2*m],
|
||||
[0, 0, l**2*m*cos(q2) + l**2*m, l**2*m]])
|
||||
assert trigsimp(method.forcing_full) == trigsimp(Matrix([[u1], [u2], [-g*l*m*(sin(q1 + q2) + sin(q1)) -
|
||||
g*l*m*sin(q1) + l**2*m*(2*u1 + u2)*u2*sin(q2)],
|
||||
[-g*l*m*sin(q1 + q2) - l**2*m*u1**2*sin(q2)]]))
|
||||
|
||||
def test_two_dof_joints():
|
||||
q1, q2, u1, u2 = dynamicsymbols('q1 q2 u1 u2')
|
||||
m, c1, c2, k1, k2 = symbols('m c1 c2 k1 k2')
|
||||
with warns_deprecated_sympy():
|
||||
W = Body('W')
|
||||
B1 = Body('B1', mass=m)
|
||||
B2 = Body('B2', mass=m)
|
||||
J1 = PrismaticJoint('J1', W, B1, coordinates=q1, speeds=u1)
|
||||
J2 = PrismaticJoint('J2', B1, B2, coordinates=q2, speeds=u2)
|
||||
W.apply_force(k1*q1*W.x, reaction_body=B1)
|
||||
W.apply_force(c1*u1*W.x, reaction_body=B1)
|
||||
B1.apply_force(k2*q2*W.x, reaction_body=B2)
|
||||
B1.apply_force(c2*u2*W.x, reaction_body=B2)
|
||||
with warns_deprecated_sympy():
|
||||
method = JointsMethod(W, J1, J2)
|
||||
method.form_eoms()
|
||||
MM = method.mass_matrix
|
||||
forcing = method.forcing
|
||||
rhs = MM.LUsolve(forcing)
|
||||
assert expand(rhs[0]) == expand((-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2)/m)
|
||||
assert expand(rhs[1]) == expand((k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 *
|
||||
c2 * u2) / m)
|
||||
|
||||
def test_simple_pedulum():
|
||||
l, m, g = symbols('l m g')
|
||||
with warns_deprecated_sympy():
|
||||
C = Body('C')
|
||||
b = Body('b', mass=m)
|
||||
q = dynamicsymbols('q')
|
||||
P = PinJoint('P', C, b, speeds=q.diff(t), coordinates=q,
|
||||
child_point=-l * b.x, joint_axis=C.z)
|
||||
b.potential_energy = - m * g * l * cos(q)
|
||||
with warns_deprecated_sympy():
|
||||
method = JointsMethod(C, P)
|
||||
method.form_eoms(LagrangesMethod)
|
||||
rhs = method.rhs()
|
||||
assert rhs[1] == -g*sin(q)/l
|
||||
|
||||
def test_chaos_pendulum():
|
||||
#https://www.pydy.org/examples/chaos_pendulum.html
|
||||
mA, mB, lA, lB, IAxx, IBxx, IByy, IBzz, g = symbols('mA, mB, lA, lB, IAxx, IBxx, IByy, IBzz, g')
|
||||
theta, phi, omega, alpha = dynamicsymbols('theta phi omega alpha')
|
||||
|
||||
A = ReferenceFrame('A')
|
||||
B = ReferenceFrame('B')
|
||||
|
||||
with warns_deprecated_sympy():
|
||||
rod = Body('rod', mass=mA, frame=A,
|
||||
central_inertia=inertia(A, IAxx, IAxx, 0))
|
||||
plate = Body('plate', mass=mB, frame=B,
|
||||
central_inertia=inertia(B, IBxx, IByy, IBzz))
|
||||
C = Body('C')
|
||||
J1 = PinJoint('J1', C, rod, coordinates=theta, speeds=omega,
|
||||
child_point=-lA * rod.z, joint_axis=C.y)
|
||||
J2 = PinJoint('J2', rod, plate, coordinates=phi, speeds=alpha,
|
||||
parent_point=(lB - lA) * rod.z, joint_axis=rod.z)
|
||||
|
||||
rod.apply_force(mA*g*C.z)
|
||||
plate.apply_force(mB*g*C.z)
|
||||
|
||||
with warns_deprecated_sympy():
|
||||
method = JointsMethod(C, J1, J2)
|
||||
method.form_eoms()
|
||||
|
||||
MM = method.mass_matrix
|
||||
forcing = method.forcing
|
||||
rhs = MM.LUsolve(forcing)
|
||||
xd = (-2 * IBxx * alpha * omega * sin(phi) * cos(phi) + 2 * IByy * alpha * omega * sin(phi) *
|
||||
cos(phi) - g * lA * mA * sin(theta) - g * lB * mB * sin(theta)) / (IAxx + IBxx *
|
||||
sin(phi)**2 + IByy * cos(phi)**2 + lA**2 * mA + lB**2 * mB)
|
||||
assert (rhs[0] - xd).simplify() == 0
|
||||
xd = (IBxx - IByy) * omega**2 * sin(phi) * cos(phi) / IBzz
|
||||
assert (rhs[1] - xd).simplify() == 0
|
||||
|
||||
def test_four_bar_linkage_with_manual_constraints():
|
||||
q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1:4, u1:4')
|
||||
l1, l2, l3, l4, rho = symbols('l1:5, rho')
|
||||
|
||||
N = ReferenceFrame('N')
|
||||
inertias = [inertia(N, 0, 0, rho * l ** 3 / 12) for l in (l1, l2, l3, l4)]
|
||||
with warns_deprecated_sympy():
|
||||
link1 = Body('Link1', frame=N, mass=rho * l1,
|
||||
central_inertia=inertias[0])
|
||||
link2 = Body('Link2', mass=rho * l2, central_inertia=inertias[1])
|
||||
link3 = Body('Link3', mass=rho * l3, central_inertia=inertias[2])
|
||||
link4 = Body('Link4', mass=rho * l4, central_inertia=inertias[3])
|
||||
|
||||
joint1 = PinJoint(
|
||||
'J1', link1, link2, coordinates=q1, speeds=u1, joint_axis=link1.z,
|
||||
parent_point=l1 / 2 * link1.x, child_point=-l2 / 2 * link2.x)
|
||||
joint2 = PinJoint(
|
||||
'J2', link2, link3, coordinates=q2, speeds=u2, joint_axis=link2.z,
|
||||
parent_point=l2 / 2 * link2.x, child_point=-l3 / 2 * link3.x)
|
||||
joint3 = PinJoint(
|
||||
'J3', link3, link4, coordinates=q3, speeds=u3, joint_axis=link3.z,
|
||||
parent_point=l3 / 2 * link3.x, child_point=-l4 / 2 * link4.x)
|
||||
|
||||
loop = link4.masscenter.pos_from(link1.masscenter) \
|
||||
+ l1 / 2 * link1.x + l4 / 2 * link4.x
|
||||
|
||||
fh = Matrix([loop.dot(link1.x), loop.dot(link1.y)])
|
||||
|
||||
with warns_deprecated_sympy():
|
||||
method = JointsMethod(link1, joint1, joint2, joint3)
|
||||
|
||||
t = dynamicsymbols._t
|
||||
qdots = solve(method.kdes, [q1.diff(t), q2.diff(t), q3.diff(t)])
|
||||
fhd = fh.diff(t).subs(qdots)
|
||||
|
||||
kane = KanesMethod(method.frame, q_ind=[q1], u_ind=[u1],
|
||||
q_dependent=[q2, q3], u_dependent=[u2, u3],
|
||||
kd_eqs=method.kdes, configuration_constraints=fh,
|
||||
velocity_constraints=fhd, forcelist=method.loads,
|
||||
bodies=method.bodies)
|
||||
fr, frs = kane.kanes_equations()
|
||||
assert fr == zeros(1)
|
||||
|
||||
# Numerically check the mass- and forcing-matrix
|
||||
p = Matrix([l1, l2, l3, l4, rho])
|
||||
q = Matrix([q1, q2, q3])
|
||||
u = Matrix([u1, u2, u3])
|
||||
eval_m = lambdify((q, p), kane.mass_matrix)
|
||||
eval_f = lambdify((q, u, p), kane.forcing)
|
||||
eval_fhd = lambdify((q, u, p), fhd)
|
||||
|
||||
p_vals = [0.13, 0.24, 0.21, 0.34, 997]
|
||||
q_vals = [2.1, 0.6655470375077588, 2.527408138024188] # Satisfies fh
|
||||
u_vals = [0.2, -0.17963733938852067, 0.1309060540601612] # Satisfies fhd
|
||||
mass_check = Matrix([[3.452709815256506e+01, 7.003948798374735e+00,
|
||||
-4.939690970641498e+00],
|
||||
[-2.203792703880936e-14, 2.071702479957077e-01,
|
||||
2.842917573033711e-01],
|
||||
[-1.300000000000123e-01, -8.836934896046506e-03,
|
||||
1.864891330060847e-01]])
|
||||
forcing_check = Matrix([[-0.031211821321648],
|
||||
[-0.00066022608181],
|
||||
[0.001813559741243]])
|
||||
eps = 1e-10
|
||||
assert all(abs(x) < eps for x in eval_fhd(q_vals, u_vals, p_vals))
|
||||
assert all(abs(x) < eps for x in
|
||||
(Matrix(eval_m(q_vals, p_vals)) - mass_check))
|
||||
assert all(abs(x) < eps for x in
|
||||
(Matrix(eval_f(q_vals, u_vals, p_vals)) - forcing_check))
|
||||
+553
@@ -0,0 +1,553 @@
|
||||
from sympy import solve
|
||||
from sympy import (cos, expand, Matrix, sin, symbols, tan, sqrt, S,
|
||||
zeros, eye)
|
||||
from sympy.simplify.simplify import simplify
|
||||
from sympy.physics.mechanics import (dynamicsymbols, ReferenceFrame, Point,
|
||||
RigidBody, KanesMethod, inertia, Particle,
|
||||
dot, find_dynamicsymbols)
|
||||
from sympy.testing.pytest import raises
|
||||
|
||||
|
||||
def test_invalid_coordinates():
|
||||
# Simple pendulum, but use symbols instead of dynamicsymbols
|
||||
l, m, g = symbols('l m g')
|
||||
q, u = symbols('q u') # Generalized coordinate
|
||||
kd = [q.diff(dynamicsymbols._t) - u]
|
||||
N, O = ReferenceFrame('N'), Point('O')
|
||||
O.set_vel(N, 0)
|
||||
P = Particle('P', Point('P'), m)
|
||||
P.point.set_pos(O, l * (sin(q) * N.x - cos(q) * N.y))
|
||||
F = (P.point, -m * g * N.y)
|
||||
raises(ValueError, lambda: KanesMethod(N, [q], [u], kd, bodies=[P],
|
||||
forcelist=[F]))
|
||||
|
||||
|
||||
def test_one_dof():
|
||||
# This is for a 1 dof spring-mass-damper case.
|
||||
# It is described in more detail in the KanesMethod docstring.
|
||||
q, u = dynamicsymbols('q u')
|
||||
qd, ud = dynamicsymbols('q u', 1)
|
||||
m, c, k = symbols('m c k')
|
||||
N = ReferenceFrame('N')
|
||||
P = Point('P')
|
||||
P.set_vel(N, u * N.x)
|
||||
|
||||
kd = [qd - u]
|
||||
FL = [(P, (-k * q - c * u) * N.x)]
|
||||
pa = Particle('pa', P, m)
|
||||
BL = [pa]
|
||||
|
||||
KM = KanesMethod(N, [q], [u], kd)
|
||||
KM.kanes_equations(BL, FL)
|
||||
|
||||
assert KM.bodies == BL
|
||||
assert KM.loads == FL
|
||||
|
||||
MM = KM.mass_matrix
|
||||
forcing = KM.forcing
|
||||
rhs = MM.inv() * forcing
|
||||
assert expand(rhs[0]) == expand(-(q * k + u * c) / m)
|
||||
|
||||
assert simplify(KM.rhs() -
|
||||
KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(2, 1)
|
||||
|
||||
assert (KM.linearize(A_and_B=True, )[0] == Matrix([[0, 1], [-k/m, -c/m]]))
|
||||
|
||||
|
||||
def test_two_dof():
|
||||
# This is for a 2 d.o.f., 2 particle spring-mass-damper.
|
||||
# The first coordinate is the displacement of the first particle, and the
|
||||
# second is the relative displacement between the first and second
|
||||
# particles. Speeds are defined as the time derivatives of the particles.
|
||||
q1, q2, u1, u2 = dynamicsymbols('q1 q2 u1 u2')
|
||||
q1d, q2d, u1d, u2d = dynamicsymbols('q1 q2 u1 u2', 1)
|
||||
m, c1, c2, k1, k2 = symbols('m c1 c2 k1 k2')
|
||||
N = ReferenceFrame('N')
|
||||
P1 = Point('P1')
|
||||
P2 = Point('P2')
|
||||
P1.set_vel(N, u1 * N.x)
|
||||
P2.set_vel(N, (u1 + u2) * N.x)
|
||||
# Note we multiply the kinematic equation by an arbitrary factor
|
||||
# to test the implicit vs explicit kinematics attribute
|
||||
kd = [q1d/2 - u1/2, 2*q2d - 2*u2]
|
||||
|
||||
# Now we create the list of forces, then assign properties to each
|
||||
# particle, then create a list of all particles.
|
||||
FL = [(P1, (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) * N.x), (P2, (-k2 *
|
||||
q2 - c2 * u2) * N.x)]
|
||||
pa1 = Particle('pa1', P1, m)
|
||||
pa2 = Particle('pa2', P2, m)
|
||||
BL = [pa1, pa2]
|
||||
|
||||
# Finally we create the KanesMethod object, specify the inertial frame,
|
||||
# pass relevant information, and form Fr & Fr*. Then we calculate the mass
|
||||
# matrix and forcing terms, and finally solve for the udots.
|
||||
KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd)
|
||||
KM.kanes_equations(BL, FL)
|
||||
MM = KM.mass_matrix
|
||||
forcing = KM.forcing
|
||||
rhs = MM.inv() * forcing
|
||||
assert expand(rhs[0]) == expand((-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2)/m)
|
||||
assert expand(rhs[1]) == expand((k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 *
|
||||
c2 * u2) / m)
|
||||
|
||||
# Check that the explicit form is the default and kinematic mass matrix is identity
|
||||
assert KM.explicit_kinematics
|
||||
assert KM.mass_matrix_kin == eye(2)
|
||||
|
||||
# Check that for the implicit form the mass matrix is not identity
|
||||
KM.explicit_kinematics = False
|
||||
assert KM.mass_matrix_kin == Matrix([[S(1)/2, 0], [0, 2]])
|
||||
|
||||
# Check that whether using implicit or explicit kinematics the RHS
|
||||
# equations are consistent with the matrix form
|
||||
for explicit_kinematics in [False, True]:
|
||||
KM.explicit_kinematics = explicit_kinematics
|
||||
assert simplify(KM.rhs() -
|
||||
KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(4, 1)
|
||||
|
||||
# Make sure an error is raised if nonlinear kinematic differential
|
||||
# equations are supplied.
|
||||
kd = [q1d - u1**2, sin(q2d) - cos(u2)]
|
||||
raises(ValueError, lambda: KanesMethod(N, q_ind=[q1, q2],
|
||||
u_ind=[u1, u2], kd_eqs=kd))
|
||||
|
||||
def test_pend():
|
||||
q, u = dynamicsymbols('q u')
|
||||
qd, ud = dynamicsymbols('q u', 1)
|
||||
m, l, g = symbols('m l g')
|
||||
N = ReferenceFrame('N')
|
||||
P = Point('P')
|
||||
P.set_vel(N, -l * u * sin(q) * N.x + l * u * cos(q) * N.y)
|
||||
kd = [qd - u]
|
||||
|
||||
FL = [(P, m * g * N.x)]
|
||||
pa = Particle('pa', P, m)
|
||||
BL = [pa]
|
||||
|
||||
KM = KanesMethod(N, [q], [u], kd)
|
||||
KM.kanes_equations(BL, FL)
|
||||
MM = KM.mass_matrix
|
||||
forcing = KM.forcing
|
||||
rhs = MM.inv() * forcing
|
||||
rhs.simplify()
|
||||
assert expand(rhs[0]) == expand(-g / l * sin(q))
|
||||
assert simplify(KM.rhs() -
|
||||
KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(2, 1)
|
||||
|
||||
|
||||
def test_rolling_disc():
|
||||
# Rolling Disc Example
|
||||
# Here the rolling disc is formed from the contact point up, removing the
|
||||
# need to introduce generalized speeds. Only 3 configuration and three
|
||||
# speed variables are need to describe this system, along with the disc's
|
||||
# mass and radius, and the local gravity (note that mass will drop out).
|
||||
q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3')
|
||||
q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
|
||||
r, m, g = symbols('r m g')
|
||||
|
||||
# The kinematics are formed by a series of simple rotations. Each simple
|
||||
# rotation creates a new frame, and the next rotation is defined by the new
|
||||
# frame's basis vectors. This example uses a 3-1-2 series of rotations, or
|
||||
# Z, X, Y series of rotations. Angular velocity for this is defined using
|
||||
# the second frame's basis (the lean frame).
|
||||
N = ReferenceFrame('N')
|
||||
Y = N.orientnew('Y', 'Axis', [q1, N.z])
|
||||
L = Y.orientnew('L', 'Axis', [q2, Y.x])
|
||||
R = L.orientnew('R', 'Axis', [q3, L.y])
|
||||
w_R_N_qd = R.ang_vel_in(N)
|
||||
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
|
||||
|
||||
# This is the translational kinematics. We create a point with no velocity
|
||||
# in N; this is the contact point between the disc and ground. Next we form
|
||||
# the position vector from the contact point to the disc's center of mass.
|
||||
# Finally we form the velocity and acceleration of the disc.
|
||||
C = Point('C')
|
||||
C.set_vel(N, 0)
|
||||
Dmc = C.locatenew('Dmc', r * L.z)
|
||||
Dmc.v2pt_theory(C, N, R)
|
||||
|
||||
# This is a simple way to form the inertia dyadic.
|
||||
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
|
||||
|
||||
# Kinematic differential equations; how the generalized coordinate time
|
||||
# derivatives relate to generalized speeds.
|
||||
kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L]
|
||||
|
||||
# Creation of the force list; it is the gravitational force at the mass
|
||||
# center of the disc. Then we create the disc by assigning a Point to the
|
||||
# center of mass attribute, a ReferenceFrame to the frame attribute, and mass
|
||||
# and inertia. Then we form the body list.
|
||||
ForceList = [(Dmc, - m * g * Y.z)]
|
||||
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
|
||||
BodyList = [BodyD]
|
||||
|
||||
# Finally we form the equations of motion, using the same steps we did
|
||||
# before. Specify inertial frame, supply generalized speeds, supply
|
||||
# kinematic differential equation dictionary, compute Fr from the force
|
||||
# list and Fr* from the body list, compute the mass matrix and forcing
|
||||
# terms, then solve for the u dots (time derivatives of the generalized
|
||||
# speeds).
|
||||
KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd)
|
||||
KM.kanes_equations(BodyList, ForceList)
|
||||
MM = KM.mass_matrix
|
||||
forcing = KM.forcing
|
||||
rhs = MM.inv() * forcing
|
||||
kdd = KM.kindiffdict()
|
||||
rhs = rhs.subs(kdd)
|
||||
rhs.simplify()
|
||||
assert rhs.expand() == Matrix([(6*u2*u3*r - u3**2*r*tan(q2) +
|
||||
4*g*sin(q2))/(5*r), -2*u1*u3/3, u1*(-2*u2 + u3*tan(q2))]).expand()
|
||||
assert simplify(KM.rhs() -
|
||||
KM.mass_matrix_full.LUsolve(KM.forcing_full)) == zeros(6, 1)
|
||||
|
||||
# This code tests our output vs. benchmark values. When r=g=m=1, the
|
||||
# critical speed (where all eigenvalues of the linearized equations are 0)
|
||||
# is 1 / sqrt(3) for the upright case.
|
||||
A = KM.linearize(A_and_B=True)[0]
|
||||
A_upright = A.subs({r: 1, g: 1, m: 1}).subs({q1: 0, q2: 0, q3: 0, u1: 0, u3: 0})
|
||||
import sympy
|
||||
assert sympy.sympify(A_upright.subs({u2: 1 / sqrt(3)})).eigenvals() == {S.Zero: 6}
|
||||
|
||||
|
||||
def test_aux():
|
||||
# Same as above, except we have 2 auxiliary speeds for the ground contact
|
||||
# point, which is known to be zero. In one case, we go through then
|
||||
# substitute the aux. speeds in at the end (they are zero, as well as their
|
||||
# derivative), in the other case, we use the built-in auxiliary speed part
|
||||
# of KanesMethod. The equations from each should be the same.
|
||||
q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1 q2 q3 u1 u2 u3')
|
||||
q1d, q2d, q3d, u1d, u2d, u3d = dynamicsymbols('q1 q2 q3 u1 u2 u3', 1)
|
||||
u4, u5, f1, f2 = dynamicsymbols('u4, u5, f1, f2')
|
||||
u4d, u5d = dynamicsymbols('u4, u5', 1)
|
||||
r, m, g = symbols('r m g')
|
||||
|
||||
N = ReferenceFrame('N')
|
||||
Y = N.orientnew('Y', 'Axis', [q1, N.z])
|
||||
L = Y.orientnew('L', 'Axis', [q2, Y.x])
|
||||
R = L.orientnew('R', 'Axis', [q3, L.y])
|
||||
w_R_N_qd = R.ang_vel_in(N)
|
||||
R.set_ang_vel(N, u1 * L.x + u2 * L.y + u3 * L.z)
|
||||
|
||||
C = Point('C')
|
||||
C.set_vel(N, u4 * L.x + u5 * (Y.z ^ L.x))
|
||||
Dmc = C.locatenew('Dmc', r * L.z)
|
||||
Dmc.v2pt_theory(C, N, R)
|
||||
Dmc.a2pt_theory(C, N, R)
|
||||
|
||||
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
|
||||
|
||||
kd = [dot(R.ang_vel_in(N) - w_R_N_qd, uv) for uv in L]
|
||||
|
||||
ForceList = [(Dmc, - m * g * Y.z), (C, f1 * L.x + f2 * (Y.z ^ L.x))]
|
||||
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
|
||||
BodyList = [BodyD]
|
||||
|
||||
KM = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3, u4, u5],
|
||||
kd_eqs=kd)
|
||||
(fr, frstar) = KM.kanes_equations(BodyList, ForceList)
|
||||
fr = fr.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
|
||||
frstar = frstar.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
|
||||
|
||||
KM2 = KanesMethod(N, q_ind=[q1, q2, q3], u_ind=[u1, u2, u3], kd_eqs=kd,
|
||||
u_auxiliary=[u4, u5])
|
||||
(fr2, frstar2) = KM2.kanes_equations(BodyList, ForceList)
|
||||
fr2 = fr2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
|
||||
frstar2 = frstar2.subs({u4d: 0, u5d: 0}).subs({u4: 0, u5: 0})
|
||||
|
||||
frstar.simplify()
|
||||
frstar2.simplify()
|
||||
|
||||
assert (fr - fr2).expand() == Matrix([0, 0, 0, 0, 0])
|
||||
assert (frstar - frstar2).expand() == Matrix([0, 0, 0, 0, 0])
|
||||
|
||||
|
||||
def test_parallel_axis():
|
||||
# This is for a 2 dof inverted pendulum on a cart.
|
||||
# This tests the parallel axis code in KanesMethod. The inertia of the
|
||||
# pendulum is defined about the hinge, not about the center of mass.
|
||||
|
||||
# Defining the constants and knowns of the system
|
||||
gravity = symbols('g')
|
||||
k, ls = symbols('k ls')
|
||||
a, mA, mC = symbols('a mA mC')
|
||||
F = dynamicsymbols('F')
|
||||
Ix, Iy, Iz = symbols('Ix Iy Iz')
|
||||
|
||||
# Declaring the Generalized coordinates and speeds
|
||||
q1, q2 = dynamicsymbols('q1 q2')
|
||||
q1d, q2d = dynamicsymbols('q1 q2', 1)
|
||||
u1, u2 = dynamicsymbols('u1 u2')
|
||||
u1d, u2d = dynamicsymbols('u1 u2', 1)
|
||||
|
||||
# Creating reference frames
|
||||
N = ReferenceFrame('N')
|
||||
A = ReferenceFrame('A')
|
||||
|
||||
A.orient(N, 'Axis', [-q2, N.z])
|
||||
A.set_ang_vel(N, -u2 * N.z)
|
||||
|
||||
# Origin of Newtonian reference frame
|
||||
O = Point('O')
|
||||
|
||||
# Creating and Locating the positions of the cart, C, and the
|
||||
# center of mass of the pendulum, A
|
||||
C = O.locatenew('C', q1 * N.x)
|
||||
Ao = C.locatenew('Ao', a * A.y)
|
||||
|
||||
# Defining velocities of the points
|
||||
O.set_vel(N, 0)
|
||||
C.set_vel(N, u1 * N.x)
|
||||
Ao.v2pt_theory(C, N, A)
|
||||
Cart = Particle('Cart', C, mC)
|
||||
Pendulum = RigidBody('Pendulum', Ao, A, mA, (inertia(A, Ix, Iy, Iz), C))
|
||||
|
||||
# kinematical differential equations
|
||||
|
||||
kindiffs = [q1d - u1, q2d - u2]
|
||||
|
||||
bodyList = [Cart, Pendulum]
|
||||
|
||||
forceList = [(Ao, -N.y * gravity * mA),
|
||||
(C, -N.y * gravity * mC),
|
||||
(C, -N.x * k * (q1 - ls)),
|
||||
(C, N.x * F)]
|
||||
|
||||
km = KanesMethod(N, [q1, q2], [u1, u2], kindiffs)
|
||||
(fr, frstar) = km.kanes_equations(bodyList, forceList)
|
||||
mm = km.mass_matrix_full
|
||||
assert mm[3, 3] == Iz
|
||||
|
||||
def test_input_format():
|
||||
# 1 dof problem from test_one_dof
|
||||
q, u = dynamicsymbols('q u')
|
||||
qd, ud = dynamicsymbols('q u', 1)
|
||||
m, c, k = symbols('m c k')
|
||||
N = ReferenceFrame('N')
|
||||
P = Point('P')
|
||||
P.set_vel(N, u * N.x)
|
||||
|
||||
kd = [qd - u]
|
||||
FL = [(P, (-k * q - c * u) * N.x)]
|
||||
pa = Particle('pa', P, m)
|
||||
BL = [pa]
|
||||
|
||||
KM = KanesMethod(N, [q], [u], kd)
|
||||
# test for input format kane.kanes_equations((body1, body2, particle1))
|
||||
assert KM.kanes_equations(BL)[0] == Matrix([0])
|
||||
# test for input format kane.kanes_equations(bodies=(body1, body 2), loads=(load1,load2))
|
||||
assert KM.kanes_equations(bodies=BL, loads=None)[0] == Matrix([0])
|
||||
# test for input format kane.kanes_equations(bodies=(body1, body 2), loads=None)
|
||||
assert KM.kanes_equations(BL, loads=None)[0] == Matrix([0])
|
||||
# test for input format kane.kanes_equations(bodies=(body1, body 2))
|
||||
assert KM.kanes_equations(BL)[0] == Matrix([0])
|
||||
# test for input format kane.kanes_equations(bodies=(body1, body2), loads=[])
|
||||
assert KM.kanes_equations(BL, [])[0] == Matrix([0])
|
||||
# test for error raised when a wrong force list (in this case a string) is provided
|
||||
raises(ValueError, lambda: KM._form_fr('bad input'))
|
||||
|
||||
# 1 dof problem from test_one_dof with FL & BL in instance
|
||||
KM = KanesMethod(N, [q], [u], kd, bodies=BL, forcelist=FL)
|
||||
assert KM.kanes_equations()[0] == Matrix([-c*u - k*q])
|
||||
|
||||
# 2 dof problem from test_two_dof
|
||||
q1, q2, u1, u2 = dynamicsymbols('q1 q2 u1 u2')
|
||||
q1d, q2d, u1d, u2d = dynamicsymbols('q1 q2 u1 u2', 1)
|
||||
m, c1, c2, k1, k2 = symbols('m c1 c2 k1 k2')
|
||||
N = ReferenceFrame('N')
|
||||
P1 = Point('P1')
|
||||
P2 = Point('P2')
|
||||
P1.set_vel(N, u1 * N.x)
|
||||
P2.set_vel(N, (u1 + u2) * N.x)
|
||||
kd = [q1d - u1, q2d - u2]
|
||||
|
||||
FL = ((P1, (-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2) * N.x), (P2, (-k2 *
|
||||
q2 - c2 * u2) * N.x))
|
||||
pa1 = Particle('pa1', P1, m)
|
||||
pa2 = Particle('pa2', P2, m)
|
||||
BL = (pa1, pa2)
|
||||
|
||||
KM = KanesMethod(N, q_ind=[q1, q2], u_ind=[u1, u2], kd_eqs=kd)
|
||||
# test for input format
|
||||
# kane.kanes_equations((body1, body2), (load1, load2))
|
||||
KM.kanes_equations(BL, FL)
|
||||
MM = KM.mass_matrix
|
||||
forcing = KM.forcing
|
||||
rhs = MM.inv() * forcing
|
||||
assert expand(rhs[0]) == expand((-k1 * q1 - c1 * u1 + k2 * q2 + c2 * u2)/m)
|
||||
assert expand(rhs[1]) == expand((k1 * q1 + c1 * u1 - 2 * k2 * q2 - 2 *
|
||||
c2 * u2) / m)
|
||||
|
||||
|
||||
def test_implicit_kinematics():
|
||||
# Test that implicit kinematics can handle complicated
|
||||
# equations that explicit form struggles with
|
||||
# See https://github.com/sympy/sympy/issues/22626
|
||||
|
||||
# Inertial frame
|
||||
NED = ReferenceFrame('NED')
|
||||
NED_o = Point('NED_o')
|
||||
NED_o.set_vel(NED, 0)
|
||||
|
||||
# body frame
|
||||
q_att = dynamicsymbols('lambda_0:4', real=True)
|
||||
B = NED.orientnew('B', 'Quaternion', q_att)
|
||||
|
||||
# Generalized coordinates
|
||||
q_pos = dynamicsymbols('B_x:z')
|
||||
B_cm = NED_o.locatenew('B_cm', q_pos[0]*B.x + q_pos[1]*B.y + q_pos[2]*B.z)
|
||||
|
||||
q_ind = q_att[1:] + q_pos
|
||||
q_dep = [q_att[0]]
|
||||
|
||||
kinematic_eqs = []
|
||||
|
||||
# Generalized velocities
|
||||
B_ang_vel = B.ang_vel_in(NED)
|
||||
P, Q, R = dynamicsymbols('P Q R')
|
||||
B.set_ang_vel(NED, P*B.x + Q*B.y + R*B.z)
|
||||
|
||||
B_ang_vel_kd = (B.ang_vel_in(NED) - B_ang_vel).simplify()
|
||||
|
||||
# Equating the two gives us the kinematic equation
|
||||
kinematic_eqs += [
|
||||
B_ang_vel_kd & B.x,
|
||||
B_ang_vel_kd & B.y,
|
||||
B_ang_vel_kd & B.z
|
||||
]
|
||||
|
||||
B_cm_vel = B_cm.vel(NED)
|
||||
U, V, W = dynamicsymbols('U V W')
|
||||
B_cm.set_vel(NED, U*B.x + V*B.y + W*B.z)
|
||||
|
||||
# Compute the velocity of the point using the two methods
|
||||
B_ref_vel_kd = (B_cm.vel(NED) - B_cm_vel)
|
||||
|
||||
# taking dot product with unit vectors to get kinematic equations
|
||||
# relating body coordinates and velocities
|
||||
|
||||
# Note, there is a choice to dot with NED.xyz here. That makes
|
||||
# the implicit form have some bigger terms but is still fine, the
|
||||
# explicit form still struggles though
|
||||
kinematic_eqs += [
|
||||
B_ref_vel_kd & B.x,
|
||||
B_ref_vel_kd & B.y,
|
||||
B_ref_vel_kd & B.z,
|
||||
]
|
||||
|
||||
u_ind = [U, V, W, P, Q, R]
|
||||
|
||||
# constraints
|
||||
q_att_vec = Matrix(q_att)
|
||||
config_cons = [(q_att_vec.T*q_att_vec)[0] - 1] #unit norm
|
||||
kinematic_eqs = kinematic_eqs + [(q_att_vec.T * q_att_vec.diff())[0]]
|
||||
|
||||
try:
|
||||
KM = KanesMethod(NED, q_ind, u_ind,
|
||||
q_dependent= q_dep,
|
||||
kd_eqs = kinematic_eqs,
|
||||
configuration_constraints = config_cons,
|
||||
velocity_constraints= [],
|
||||
u_dependent= [], #no dependent speeds
|
||||
u_auxiliary = [], # No auxiliary speeds
|
||||
explicit_kinematics = False # implicit kinematics
|
||||
)
|
||||
except Exception as e:
|
||||
raise e
|
||||
|
||||
# mass and inertia dyadic relative to CM
|
||||
M_B = symbols('M_B')
|
||||
J_B = inertia(B, *[S(f'J_B_{ax}')*(1 if ax[0] == ax[1] else -1)
|
||||
for ax in ['xx', 'yy', 'zz', 'xy', 'yz', 'xz']])
|
||||
J_B = J_B.subs({S('J_B_xy'): 0, S('J_B_yz'): 0})
|
||||
RB = RigidBody('RB', B_cm, B, M_B, (J_B, B_cm))
|
||||
|
||||
rigid_bodies = [RB]
|
||||
# Forces
|
||||
force_list = [
|
||||
#gravity pointing down
|
||||
(RB.masscenter, RB.mass*S('g')*NED.z),
|
||||
#generic forces and torques in body frame(inputs)
|
||||
(RB.frame, dynamicsymbols('T_z')*B.z),
|
||||
(RB.masscenter, dynamicsymbols('F_z')*B.z)
|
||||
]
|
||||
|
||||
KM.kanes_equations(rigid_bodies, force_list)
|
||||
|
||||
# Expecting implicit form to be less than 5% of the flops
|
||||
n_ops_implicit = sum(
|
||||
[x.count_ops() for x in KM.forcing_full] +
|
||||
[x.count_ops() for x in KM.mass_matrix_full]
|
||||
)
|
||||
# Save implicit kinematic matrices to use later
|
||||
mass_matrix_kin_implicit = KM.mass_matrix_kin
|
||||
forcing_kin_implicit = KM.forcing_kin
|
||||
|
||||
KM.explicit_kinematics = True
|
||||
n_ops_explicit = sum(
|
||||
[x.count_ops() for x in KM.forcing_full] +
|
||||
[x.count_ops() for x in KM.mass_matrix_full]
|
||||
)
|
||||
forcing_kin_explicit = KM.forcing_kin
|
||||
|
||||
assert n_ops_implicit / n_ops_explicit < .05
|
||||
|
||||
# Ideally we would check that implicit and explicit equations give the same result as done in test_one_dof
|
||||
# But the whole raison-d'etre of the implicit equations is to deal with problems such
|
||||
# as this one where the explicit form is too complicated to handle, especially the angular part
|
||||
# (i.e. tests would be too slow)
|
||||
# Instead, we check that the kinematic equations are correct using more fundamental tests:
|
||||
#
|
||||
# (1) that we recover the kinematic equations we have provided
|
||||
assert (mass_matrix_kin_implicit * KM.q.diff() - forcing_kin_implicit) == Matrix(kinematic_eqs)
|
||||
|
||||
# (2) that rate of quaternions matches what 'textbook' solutions give
|
||||
# Note that we just use the explicit kinematics for the linear velocities
|
||||
# as they are not as complicated as the angular ones
|
||||
qdot_candidate = forcing_kin_explicit
|
||||
|
||||
quat_dot_textbook = Matrix([
|
||||
[0, -P, -Q, -R],
|
||||
[P, 0, R, -Q],
|
||||
[Q, -R, 0, P],
|
||||
[R, Q, -P, 0],
|
||||
]) * q_att_vec / 2
|
||||
|
||||
# Again, if we don't use this "textbook" solution
|
||||
# sympy will struggle to deal with the terms related to quaternion rates
|
||||
# due to the number of operations involved
|
||||
qdot_candidate[-1] = quat_dot_textbook[0] # lambda_0, note the [-1] as sympy's Kane puts the dependent coordinate last
|
||||
qdot_candidate[0] = quat_dot_textbook[1] # lambda_1
|
||||
qdot_candidate[1] = quat_dot_textbook[2] # lambda_2
|
||||
qdot_candidate[2] = quat_dot_textbook[3] # lambda_3
|
||||
|
||||
# sub the config constraint in the candidate solution and compare to the implicit rhs
|
||||
lambda_0_sol = solve(config_cons[0], q_att_vec[0])[1]
|
||||
lhs_candidate = simplify(mass_matrix_kin_implicit * qdot_candidate).subs({q_att_vec[0]: lambda_0_sol})
|
||||
assert lhs_candidate == forcing_kin_implicit
|
||||
|
||||
def test_issue_24887():
|
||||
# Spherical pendulum
|
||||
g, l, m, c = symbols('g l m c')
|
||||
q1, q2, q3, u1, u2, u3 = dynamicsymbols('q1:4 u1:4')
|
||||
N = ReferenceFrame('N')
|
||||
A = ReferenceFrame('A')
|
||||
A.orient_body_fixed(N, (q1, q2, q3), 'zxy')
|
||||
N_w_A = A.ang_vel_in(N)
|
||||
# A.set_ang_vel(N, u1 * A.x + u2 * A.y + u3 * A.z)
|
||||
kdes = [N_w_A.dot(A.x) - u1, N_w_A.dot(A.y) - u2, N_w_A.dot(A.z) - u3]
|
||||
O = Point('O')
|
||||
O.set_vel(N, 0)
|
||||
Po = O.locatenew('Po', -l * A.y)
|
||||
Po.set_vel(A, 0)
|
||||
P = Particle('P', Po, m)
|
||||
kane = KanesMethod(N, [q1, q2, q3], [u1, u2, u3], kdes, bodies=[P],
|
||||
forcelist=[(Po, -m * g * N.y)])
|
||||
kane.kanes_equations()
|
||||
expected_md = m * l ** 2 * Matrix([[1, 0, 0], [0, 0, 0], [0, 0, 1]])
|
||||
expected_fd = Matrix([
|
||||
[l*m*(g*(sin(q1)*sin(q3) - sin(q2)*cos(q1)*cos(q3)) - l*u2*u3)],
|
||||
[0], [l*m*(-g*(sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1)) + l*u1*u2)]])
|
||||
assert find_dynamicsymbols(kane.forcing).issubset({q1, q2, q3, u1, u2, u3})
|
||||
assert simplify(kane.mass_matrix - expected_md) == zeros(3, 3)
|
||||
assert simplify(kane.forcing - expected_fd) == zeros(3, 1)
|
||||
+464
@@ -0,0 +1,464 @@
|
||||
from sympy import cos, Matrix, sin, zeros, tan, pi, symbols
|
||||
from sympy.simplify.simplify import simplify
|
||||
from sympy.simplify.trigsimp import trigsimp
|
||||
from sympy.solvers.solvers import solve
|
||||
from sympy.physics.mechanics import (cross, dot, dynamicsymbols,
|
||||
find_dynamicsymbols, KanesMethod, inertia,
|
||||
inertia_of_point_mass, Point,
|
||||
ReferenceFrame, RigidBody)
|
||||
|
||||
|
||||
def test_aux_dep():
|
||||
# This test is about rolling disc dynamics, comparing the results found
|
||||
# with KanesMethod to those found when deriving the equations "manually"
|
||||
# with SymPy.
|
||||
# The terms Fr, Fr*, and Fr*_steady are all compared between the two
|
||||
# methods. Here, Fr*_steady refers to the generalized inertia forces for an
|
||||
# equilibrium configuration.
|
||||
# Note: comparing to the test of test_rolling_disc() in test_kane.py, this
|
||||
# test also tests auxiliary speeds and configuration and motion constraints
|
||||
#, seen in the generalized dependent coordinates q[3], and depend speeds
|
||||
# u[3], u[4] and u[5].
|
||||
|
||||
|
||||
# First, manual derivation of Fr, Fr_star, Fr_star_steady.
|
||||
|
||||
# Symbols for time and constant parameters.
|
||||
# Symbols for contact forces: Fx, Fy, Fz.
|
||||
t, r, m, g, I, J = symbols('t r m g I J')
|
||||
Fx, Fy, Fz = symbols('Fx Fy Fz')
|
||||
|
||||
# Configuration variables and their time derivatives:
|
||||
# q[0] -- yaw
|
||||
# q[1] -- lean
|
||||
# q[2] -- spin
|
||||
# q[3] -- dot(-r*B.z, A.z) -- distance from ground plane to disc center in
|
||||
# A.z direction
|
||||
# Generalized speeds and their time derivatives:
|
||||
# u[0] -- disc angular velocity component, disc fixed x direction
|
||||
# u[1] -- disc angular velocity component, disc fixed y direction
|
||||
# u[2] -- disc angular velocity component, disc fixed z direction
|
||||
# u[3] -- disc velocity component, A.x direction
|
||||
# u[4] -- disc velocity component, A.y direction
|
||||
# u[5] -- disc velocity component, A.z direction
|
||||
# Auxiliary generalized speeds:
|
||||
# ua[0] -- contact point auxiliary generalized speed, A.x direction
|
||||
# ua[1] -- contact point auxiliary generalized speed, A.y direction
|
||||
# ua[2] -- contact point auxiliary generalized speed, A.z direction
|
||||
q = dynamicsymbols('q:4')
|
||||
qd = [qi.diff(t) for qi in q]
|
||||
u = dynamicsymbols('u:6')
|
||||
ud = [ui.diff(t) for ui in u]
|
||||
ud_zero = dict(zip(ud, [0.]*len(ud)))
|
||||
ua = dynamicsymbols('ua:3')
|
||||
ua_zero = dict(zip(ua, [0.]*len(ua))) # noqa:F841
|
||||
|
||||
# Reference frames:
|
||||
# Yaw intermediate frame: A.
|
||||
# Lean intermediate frame: B.
|
||||
# Disc fixed frame: C.
|
||||
N = ReferenceFrame('N')
|
||||
A = N.orientnew('A', 'Axis', [q[0], N.z])
|
||||
B = A.orientnew('B', 'Axis', [q[1], A.x])
|
||||
C = B.orientnew('C', 'Axis', [q[2], B.y])
|
||||
|
||||
# Angular velocity and angular acceleration of disc fixed frame
|
||||
# u[0], u[1] and u[2] are generalized independent speeds.
|
||||
C.set_ang_vel(N, u[0]*B.x + u[1]*B.y + u[2]*B.z)
|
||||
C.set_ang_acc(N, C.ang_vel_in(N).diff(t, B)
|
||||
+ cross(B.ang_vel_in(N), C.ang_vel_in(N)))
|
||||
|
||||
# Velocity and acceleration of points:
|
||||
# Disc-ground contact point: P.
|
||||
# Center of disc: O, defined from point P with depend coordinate: q[3]
|
||||
# u[3], u[4] and u[5] are generalized dependent speeds.
|
||||
P = Point('P')
|
||||
P.set_vel(N, ua[0]*A.x + ua[1]*A.y + ua[2]*A.z)
|
||||
O = P.locatenew('O', q[3]*A.z + r*sin(q[1])*A.y)
|
||||
O.set_vel(N, u[3]*A.x + u[4]*A.y + u[5]*A.z)
|
||||
O.set_acc(N, O.vel(N).diff(t, A) + cross(A.ang_vel_in(N), O.vel(N)))
|
||||
|
||||
# Kinematic differential equations:
|
||||
# Two equalities: one is w_c_n_qd = C.ang_vel_in(N) in three coordinates
|
||||
# directions of B, for qd0, qd1 and qd2.
|
||||
# the other is v_o_n_qd = O.vel(N) in A.z direction for qd3.
|
||||
# Then, solve for dq/dt's in terms of u's: qd_kd.
|
||||
w_c_n_qd = qd[0]*A.z + qd[1]*B.x + qd[2]*B.y
|
||||
v_o_n_qd = O.pos_from(P).diff(t, A) + cross(A.ang_vel_in(N), O.pos_from(P))
|
||||
kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] +
|
||||
[dot(v_o_n_qd - O.vel(N), A.z)])
|
||||
qd_kd = solve(kindiffs, qd) # noqa:F841
|
||||
|
||||
# Values of generalized speeds during a steady turn for later substitution
|
||||
# into the Fr_star_steady.
|
||||
steady_conditions = solve(kindiffs.subs({qd[1] : 0, qd[3] : 0}), u)
|
||||
steady_conditions.update({qd[1] : 0, qd[3] : 0})
|
||||
|
||||
# Partial angular velocities and velocities.
|
||||
partial_w_C = [C.ang_vel_in(N).diff(ui, N) for ui in u + ua]
|
||||
partial_v_O = [O.vel(N).diff(ui, N) for ui in u + ua]
|
||||
partial_v_P = [P.vel(N).diff(ui, N) for ui in u + ua]
|
||||
|
||||
# Configuration constraint: f_c, the projection of radius r in A.z direction
|
||||
# is q[3].
|
||||
# Velocity constraints: f_v, for u3, u4 and u5.
|
||||
# Acceleration constraints: f_a.
|
||||
f_c = Matrix([dot(-r*B.z, A.z) - q[3]])
|
||||
f_v = Matrix([dot(O.vel(N) - (P.vel(N) + cross(C.ang_vel_in(N),
|
||||
O.pos_from(P))), ai).expand() for ai in A])
|
||||
v_o_n = cross(C.ang_vel_in(N), O.pos_from(P))
|
||||
a_o_n = v_o_n.diff(t, A) + cross(A.ang_vel_in(N), v_o_n)
|
||||
f_a = Matrix([dot(O.acc(N) - a_o_n, ai) for ai in A]) # noqa:F841
|
||||
|
||||
# Solve for constraint equations in the form of
|
||||
# u_dependent = A_rs * [u_i; u_aux].
|
||||
# First, obtain constraint coefficient matrix: M_v * [u; ua] = 0;
|
||||
# Second, taking u[0], u[1], u[2] as independent,
|
||||
# taking u[3], u[4], u[5] as dependent,
|
||||
# rearranging the matrix of M_v to be A_rs for u_dependent.
|
||||
# Third, u_aux ==0 for u_dep, and resulting dictionary of u_dep_dict.
|
||||
M_v = zeros(3, 9)
|
||||
for i in range(3):
|
||||
for j, ui in enumerate(u + ua):
|
||||
M_v[i, j] = f_v[i].diff(ui)
|
||||
|
||||
M_v_i = M_v[:, :3]
|
||||
M_v_d = M_v[:, 3:6]
|
||||
M_v_aux = M_v[:, 6:]
|
||||
M_v_i_aux = M_v_i.row_join(M_v_aux)
|
||||
A_rs = - M_v_d.inv() * M_v_i_aux
|
||||
|
||||
u_dep = A_rs[:, :3] * Matrix(u[:3])
|
||||
u_dep_dict = dict(zip(u[3:], u_dep))
|
||||
|
||||
# Active forces: F_O acting on point O; F_P acting on point P.
|
||||
# Generalized active forces (unconstrained): Fr_u = F_point * pv_point.
|
||||
F_O = m*g*A.z
|
||||
F_P = Fx * A.x + Fy * A.y + Fz * A.z
|
||||
Fr_u = Matrix([dot(F_O, pv_o) + dot(F_P, pv_p) for pv_o, pv_p in
|
||||
zip(partial_v_O, partial_v_P)])
|
||||
|
||||
# Inertia force: R_star_O.
|
||||
# Inertia of disc: I_C_O, where J is a inertia component about principal axis.
|
||||
# Inertia torque: T_star_C.
|
||||
# Generalized inertia forces (unconstrained): Fr_star_u.
|
||||
R_star_O = -m*O.acc(N)
|
||||
I_C_O = inertia(B, I, J, I)
|
||||
T_star_C = -(dot(I_C_O, C.ang_acc_in(N)) \
|
||||
+ cross(C.ang_vel_in(N), dot(I_C_O, C.ang_vel_in(N))))
|
||||
Fr_star_u = Matrix([dot(R_star_O, pv) + dot(T_star_C, pav) for pv, pav in
|
||||
zip(partial_v_O, partial_w_C)])
|
||||
|
||||
# Form nonholonomic Fr: Fr_c, and nonholonomic Fr_star: Fr_star_c.
|
||||
# Also, nonholonomic Fr_star in steady turning condition: Fr_star_steady.
|
||||
Fr_c = Fr_u[:3, :].col_join(Fr_u[6:, :]) + A_rs.T * Fr_u[3:6, :]
|
||||
Fr_star_c = Fr_star_u[:3, :].col_join(Fr_star_u[6:, :])\
|
||||
+ A_rs.T * Fr_star_u[3:6, :]
|
||||
Fr_star_steady = Fr_star_c.subs(ud_zero).subs(u_dep_dict)\
|
||||
.subs(steady_conditions).subs({q[3]: -r*cos(q[1])}).expand()
|
||||
|
||||
|
||||
# Second, using KaneMethod in mechanics for fr, frstar and frstar_steady.
|
||||
|
||||
# Rigid Bodies: disc, with inertia I_C_O.
|
||||
iner_tuple = (I_C_O, O)
|
||||
disc = RigidBody('disc', O, C, m, iner_tuple)
|
||||
bodyList = [disc]
|
||||
|
||||
# Generalized forces: Gravity: F_o; Auxiliary forces: F_p.
|
||||
F_o = (O, F_O)
|
||||
F_p = (P, F_P)
|
||||
forceList = [F_o, F_p]
|
||||
|
||||
# KanesMethod.
|
||||
kane = KanesMethod(
|
||||
N, q_ind= q[:3], u_ind= u[:3], kd_eqs=kindiffs,
|
||||
q_dependent=q[3:], configuration_constraints = f_c,
|
||||
u_dependent=u[3:], velocity_constraints= f_v,
|
||||
u_auxiliary=ua
|
||||
)
|
||||
|
||||
# fr, frstar, frstar_steady and kdd(kinematic differential equations).
|
||||
(fr, frstar)= kane.kanes_equations(bodyList, forceList)
|
||||
frstar_steady = frstar.subs(ud_zero).subs(u_dep_dict).subs(steady_conditions)\
|
||||
.subs({q[3]: -r*cos(q[1])}).expand()
|
||||
kdd = kane.kindiffdict()
|
||||
|
||||
assert Matrix(Fr_c).expand() == fr.expand()
|
||||
assert Matrix(Fr_star_c.subs(kdd)).expand() == frstar.expand()
|
||||
# These Matrices have some Integer(0) and some Float(0). Running under
|
||||
# SymEngine gives different types of zero.
|
||||
assert (simplify(Matrix(Fr_star_steady).expand()).xreplace({0:0.0}) ==
|
||||
simplify(frstar_steady.expand()).xreplace({0:0.0}))
|
||||
|
||||
syms_in_forcing = find_dynamicsymbols(kane.forcing)
|
||||
for qdi in qd:
|
||||
assert qdi not in syms_in_forcing
|
||||
|
||||
|
||||
def test_non_central_inertia():
|
||||
# This tests that the calculation of Fr* does not depend the point
|
||||
# about which the inertia of a rigid body is defined. This test solves
|
||||
# exercises 8.12, 8.17 from Kane 1985.
|
||||
|
||||
# Declare symbols
|
||||
q1, q2, q3 = dynamicsymbols('q1:4')
|
||||
q1d, q2d, q3d = dynamicsymbols('q1:4', level=1)
|
||||
u1, u2, u3, u4, u5 = dynamicsymbols('u1:6')
|
||||
u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta')
|
||||
a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t')
|
||||
Q1, Q2, Q3 = symbols('Q1, Q2 Q3')
|
||||
IA22, IA23, IA33 = symbols('IA22 IA23 IA33')
|
||||
|
||||
# Reference Frames
|
||||
F = ReferenceFrame('F')
|
||||
P = F.orientnew('P', 'axis', [-theta, F.y])
|
||||
A = P.orientnew('A', 'axis', [q1, P.x])
|
||||
A.set_ang_vel(F, u1*A.x + u3*A.z)
|
||||
# define frames for wheels
|
||||
B = A.orientnew('B', 'axis', [q2, A.z])
|
||||
C = A.orientnew('C', 'axis', [q3, A.z])
|
||||
B.set_ang_vel(A, u4 * A.z)
|
||||
C.set_ang_vel(A, u5 * A.z)
|
||||
|
||||
# define points D, S*, Q on frame A and their velocities
|
||||
pD = Point('D')
|
||||
pD.set_vel(A, 0)
|
||||
# u3 will not change v_D_F since wheels are still assumed to roll without slip.
|
||||
pD.set_vel(F, u2 * A.y)
|
||||
|
||||
pS_star = pD.locatenew('S*', e*A.y)
|
||||
pQ = pD.locatenew('Q', f*A.y - R*A.x)
|
||||
for p in [pS_star, pQ]:
|
||||
p.v2pt_theory(pD, F, A)
|
||||
|
||||
# masscenters of bodies A, B, C
|
||||
pA_star = pD.locatenew('A*', a*A.y)
|
||||
pB_star = pD.locatenew('B*', b*A.z)
|
||||
pC_star = pD.locatenew('C*', -b*A.z)
|
||||
for p in [pA_star, pB_star, pC_star]:
|
||||
p.v2pt_theory(pD, F, A)
|
||||
|
||||
# points of B, C touching the plane P
|
||||
pB_hat = pB_star.locatenew('B^', -R*A.x)
|
||||
pC_hat = pC_star.locatenew('C^', -R*A.x)
|
||||
pB_hat.v2pt_theory(pB_star, F, B)
|
||||
pC_hat.v2pt_theory(pC_star, F, C)
|
||||
|
||||
# the velocities of B^, C^ are zero since B, C are assumed to roll without slip
|
||||
kde = [q1d - u1, q2d - u4, q3d - u5]
|
||||
vc = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]]
|
||||
|
||||
# inertias of bodies A, B, C
|
||||
# IA22, IA23, IA33 are not specified in the problem statement, but are
|
||||
# necessary to define an inertia object. Although the values of
|
||||
# IA22, IA23, IA33 are not known in terms of the variables given in the
|
||||
# problem statement, they do not appear in the general inertia terms.
|
||||
inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
|
||||
inertia_B = inertia(B, K, K, J)
|
||||
inertia_C = inertia(C, K, K, J)
|
||||
|
||||
# define the rigid bodies A, B, C
|
||||
rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star))
|
||||
rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
|
||||
rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))
|
||||
|
||||
km = KanesMethod(F, q_ind=[q1, q2, q3], u_ind=[u1, u2], kd_eqs=kde,
|
||||
u_dependent=[u4, u5], velocity_constraints=vc,
|
||||
u_auxiliary=[u3])
|
||||
|
||||
forces = [(pS_star, -M*g*F.x), (pQ, Q1*A.x + Q2*A.y + Q3*A.z)]
|
||||
bodies = [rbA, rbB, rbC]
|
||||
fr, fr_star = km.kanes_equations(bodies, forces)
|
||||
vc_map = solve(vc, [u4, u5])
|
||||
|
||||
# KanesMethod returns the negative of Fr, Fr* as defined in Kane1985.
|
||||
fr_star_expected = Matrix([
|
||||
-(IA + 2*J*b**2/R**2 + 2*K +
|
||||
mA*a**2 + 2*mB*b**2) * u1.diff(t) - mA*a*u1*u2,
|
||||
-(mA + 2*mB +2*J/R**2) * u2.diff(t) + mA*a*u1**2,
|
||||
0])
|
||||
t = trigsimp(fr_star.subs(vc_map).subs({u3: 0})).doit().expand()
|
||||
assert ((fr_star_expected - t).expand() == zeros(3, 1))
|
||||
|
||||
# define inertias of rigid bodies A, B, C about point D
|
||||
# I_S/O = I_S/S* + I_S*/O
|
||||
bodies2 = []
|
||||
for rb, I_star in zip([rbA, rbB, rbC], [inertia_A, inertia_B, inertia_C]):
|
||||
I = I_star + inertia_of_point_mass(rb.mass,
|
||||
rb.masscenter.pos_from(pD),
|
||||
rb.frame)
|
||||
bodies2.append(RigidBody('', rb.masscenter, rb.frame, rb.mass,
|
||||
(I, pD)))
|
||||
fr2, fr_star2 = km.kanes_equations(bodies2, forces)
|
||||
|
||||
t = trigsimp(fr_star2.subs(vc_map).subs({u3: 0})).doit()
|
||||
assert (fr_star_expected - t).expand() == zeros(3, 1)
|
||||
|
||||
def test_sub_qdot():
|
||||
# This test solves exercises 8.12, 8.17 from Kane 1985 and defines
|
||||
# some velocities in terms of q, qdot.
|
||||
|
||||
## --- Declare symbols ---
|
||||
q1, q2, q3 = dynamicsymbols('q1:4')
|
||||
q1d, q2d, q3d = dynamicsymbols('q1:4', level=1)
|
||||
u1, u2, u3 = dynamicsymbols('u1:4')
|
||||
u_prime, R, M, g, e, f, theta = symbols('u\' R, M, g, e, f, theta')
|
||||
a, b, mA, mB, IA, J, K, t = symbols('a b mA mB IA J K t')
|
||||
IA22, IA23, IA33 = symbols('IA22 IA23 IA33')
|
||||
Q1, Q2, Q3 = symbols('Q1 Q2 Q3')
|
||||
|
||||
# --- Reference Frames ---
|
||||
F = ReferenceFrame('F')
|
||||
P = F.orientnew('P', 'axis', [-theta, F.y])
|
||||
A = P.orientnew('A', 'axis', [q1, P.x])
|
||||
A.set_ang_vel(F, u1*A.x + u3*A.z)
|
||||
# define frames for wheels
|
||||
B = A.orientnew('B', 'axis', [q2, A.z])
|
||||
C = A.orientnew('C', 'axis', [q3, A.z])
|
||||
|
||||
## --- define points D, S*, Q on frame A and their velocities ---
|
||||
pD = Point('D')
|
||||
pD.set_vel(A, 0)
|
||||
# u3 will not change v_D_F since wheels are still assumed to roll w/o slip
|
||||
pD.set_vel(F, u2 * A.y)
|
||||
|
||||
pS_star = pD.locatenew('S*', e*A.y)
|
||||
pQ = pD.locatenew('Q', f*A.y - R*A.x)
|
||||
# masscenters of bodies A, B, C
|
||||
pA_star = pD.locatenew('A*', a*A.y)
|
||||
pB_star = pD.locatenew('B*', b*A.z)
|
||||
pC_star = pD.locatenew('C*', -b*A.z)
|
||||
for p in [pS_star, pQ, pA_star, pB_star, pC_star]:
|
||||
p.v2pt_theory(pD, F, A)
|
||||
|
||||
# points of B, C touching the plane P
|
||||
pB_hat = pB_star.locatenew('B^', -R*A.x)
|
||||
pC_hat = pC_star.locatenew('C^', -R*A.x)
|
||||
pB_hat.v2pt_theory(pB_star, F, B)
|
||||
pC_hat.v2pt_theory(pC_star, F, C)
|
||||
|
||||
# --- relate qdot, u ---
|
||||
# the velocities of B^, C^ are zero since B, C are assumed to roll w/o slip
|
||||
kde = [dot(p.vel(F), A.y) for p in [pB_hat, pC_hat]]
|
||||
kde += [u1 - q1d]
|
||||
kde_map = solve(kde, [q1d, q2d, q3d])
|
||||
for k, v in list(kde_map.items()):
|
||||
kde_map[k.diff(t)] = v.diff(t)
|
||||
|
||||
# inertias of bodies A, B, C
|
||||
# IA22, IA23, IA33 are not specified in the problem statement, but are
|
||||
# necessary to define an inertia object. Although the values of
|
||||
# IA22, IA23, IA33 are not known in terms of the variables given in the
|
||||
# problem statement, they do not appear in the general inertia terms.
|
||||
inertia_A = inertia(A, IA, IA22, IA33, 0, IA23, 0)
|
||||
inertia_B = inertia(B, K, K, J)
|
||||
inertia_C = inertia(C, K, K, J)
|
||||
|
||||
# define the rigid bodies A, B, C
|
||||
rbA = RigidBody('rbA', pA_star, A, mA, (inertia_A, pA_star))
|
||||
rbB = RigidBody('rbB', pB_star, B, mB, (inertia_B, pB_star))
|
||||
rbC = RigidBody('rbC', pC_star, C, mB, (inertia_C, pC_star))
|
||||
|
||||
## --- use kanes method ---
|
||||
km = KanesMethod(F, [q1, q2, q3], [u1, u2], kd_eqs=kde, u_auxiliary=[u3])
|
||||
|
||||
forces = [(pS_star, -M*g*F.x), (pQ, Q1*A.x + Q2*A.y + Q3*A.z)]
|
||||
bodies = [rbA, rbB, rbC]
|
||||
|
||||
# Q2 = -u_prime * u2 * Q1 / sqrt(u2**2 + f**2 * u1**2)
|
||||
# -u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2) = R / Q1 * Q2
|
||||
fr_expected = Matrix([
|
||||
f*Q3 + M*g*e*sin(theta)*cos(q1),
|
||||
Q2 + M*g*sin(theta)*sin(q1),
|
||||
e*M*g*cos(theta) - Q1*f - Q2*R])
|
||||
#Q1 * (f - u_prime * R * u2 / sqrt(u2**2 + f**2 * u1**2)))])
|
||||
fr_star_expected = Matrix([
|
||||
-(IA + 2*J*b**2/R**2 + 2*K +
|
||||
mA*a**2 + 2*mB*b**2) * u1.diff(t) - mA*a*u1*u2,
|
||||
-(mA + 2*mB +2*J/R**2) * u2.diff(t) + mA*a*u1**2,
|
||||
0])
|
||||
|
||||
fr, fr_star = km.kanes_equations(bodies, forces)
|
||||
assert (fr.expand() == fr_expected.expand())
|
||||
assert ((fr_star_expected - trigsimp(fr_star)).expand() == zeros(3, 1))
|
||||
|
||||
def test_sub_qdot2():
|
||||
# This test solves exercises 8.3 from Kane 1985 and defines
|
||||
# all velocities in terms of q, qdot. We check that the generalized active
|
||||
# forces are correctly computed if u terms are only defined in the
|
||||
# kinematic differential equations.
|
||||
#
|
||||
# This functionality was added in PR 8948. Without qdot/u substitution, the
|
||||
# KanesMethod constructor will fail during the constraint initialization as
|
||||
# the B matrix will be poorly formed and inversion of the dependent part
|
||||
# will fail.
|
||||
|
||||
g, m, Px, Py, Pz, R, t = symbols('g m Px Py Pz R t')
|
||||
q = dynamicsymbols('q:5')
|
||||
qd = dynamicsymbols('q:5', level=1)
|
||||
u = dynamicsymbols('u:5')
|
||||
|
||||
## Define inertial, intermediate, and rigid body reference frames
|
||||
A = ReferenceFrame('A')
|
||||
B_prime = A.orientnew('B_prime', 'Axis', [q[0], A.z])
|
||||
B = B_prime.orientnew('B', 'Axis', [pi/2 - q[1], B_prime.x])
|
||||
C = B.orientnew('C', 'Axis', [q[2], B.z])
|
||||
|
||||
## Define points of interest and their velocities
|
||||
pO = Point('O')
|
||||
pO.set_vel(A, 0)
|
||||
|
||||
# R is the point in plane H that comes into contact with disk C.
|
||||
pR = pO.locatenew('R', q[3]*A.x + q[4]*A.y)
|
||||
pR.set_vel(A, pR.pos_from(pO).diff(t, A))
|
||||
pR.set_vel(B, 0)
|
||||
|
||||
# C^ is the point in disk C that comes into contact with plane H.
|
||||
pC_hat = pR.locatenew('C^', 0)
|
||||
pC_hat.set_vel(C, 0)
|
||||
|
||||
# C* is the point at the center of disk C.
|
||||
pCs = pC_hat.locatenew('C*', R*B.y)
|
||||
pCs.set_vel(C, 0)
|
||||
pCs.set_vel(B, 0)
|
||||
|
||||
# calculate velocites of points C* and C^ in frame A
|
||||
pCs.v2pt_theory(pR, A, B) # points C* and R are fixed in frame B
|
||||
pC_hat.v2pt_theory(pCs, A, C) # points C* and C^ are fixed in frame C
|
||||
|
||||
## Define forces on each point of the system
|
||||
R_C_hat = Px*A.x + Py*A.y + Pz*A.z
|
||||
R_Cs = -m*g*A.z
|
||||
forces = [(pC_hat, R_C_hat), (pCs, R_Cs)]
|
||||
|
||||
## Define kinematic differential equations
|
||||
# let ui = omega_C_A & bi (i = 1, 2, 3)
|
||||
# u4 = qd4, u5 = qd5
|
||||
u_expr = [C.ang_vel_in(A) & uv for uv in B]
|
||||
u_expr += qd[3:]
|
||||
kde = [ui - e for ui, e in zip(u, u_expr)]
|
||||
km1 = KanesMethod(A, q, u, kde)
|
||||
fr1, _ = km1.kanes_equations([], forces)
|
||||
|
||||
## Calculate generalized active forces if we impose the condition that the
|
||||
# disk C is rolling without slipping
|
||||
u_indep = u[:3]
|
||||
u_dep = list(set(u) - set(u_indep))
|
||||
vc = [pC_hat.vel(A) & uv for uv in [A.x, A.y]]
|
||||
km2 = KanesMethod(A, q, u_indep, kde,
|
||||
u_dependent=u_dep, velocity_constraints=vc)
|
||||
fr2, _ = km2.kanes_equations([], forces)
|
||||
|
||||
fr1_expected = Matrix([
|
||||
-R*g*m*sin(q[1]),
|
||||
-R*(Px*cos(q[0]) + Py*sin(q[0]))*tan(q[1]),
|
||||
R*(Px*cos(q[0]) + Py*sin(q[0])),
|
||||
Px,
|
||||
Py])
|
||||
fr2_expected = Matrix([
|
||||
-R*g*m*sin(q[1]),
|
||||
0,
|
||||
0])
|
||||
assert (trigsimp(fr1.expand()) == trigsimp(fr1_expected.expand()))
|
||||
assert (trigsimp(fr2.expand()) == trigsimp(fr2_expected.expand()))
|
||||
+315
@@ -0,0 +1,315 @@
|
||||
from sympy.core.numbers import pi
|
||||
from sympy.core.symbol import symbols
|
||||
from sympy.functions.elementary.miscellaneous import sqrt
|
||||
from sympy.functions.elementary.trigonometric import acos, sin, cos
|
||||
from sympy.matrices.dense import Matrix
|
||||
from sympy.physics.mechanics import (ReferenceFrame, dynamicsymbols,
|
||||
KanesMethod, inertia, Point, RigidBody,
|
||||
dot)
|
||||
from sympy.testing.pytest import slow
|
||||
|
||||
|
||||
@slow
|
||||
def test_bicycle():
|
||||
# Code to get equations of motion for a bicycle modeled as in:
|
||||
# J.P Meijaard, Jim M Papadopoulos, Andy Ruina and A.L Schwab. Linearized
|
||||
# dynamics equations for the balance and steer of a bicycle: a benchmark
|
||||
# and review. Proceedings of The Royal Society (2007) 463, 1955-1982
|
||||
# doi: 10.1098/rspa.2007.1857
|
||||
|
||||
# Note that this code has been crudely ported from Autolev, which is the
|
||||
# reason for some of the unusual naming conventions. It was purposefully as
|
||||
# similar as possible in order to aide debugging.
|
||||
|
||||
# Declare Coordinates & Speeds
|
||||
# Simple definitions for qdots - qd = u
|
||||
# Speeds are:
|
||||
# - u1: yaw frame ang. rate
|
||||
# - u2: roll frame ang. rate
|
||||
# - u3: rear wheel frame ang. rate (spinning motion)
|
||||
# - u4: frame ang. rate (pitching motion)
|
||||
# - u5: steering frame ang. rate
|
||||
# - u6: front wheel ang. rate (spinning motion)
|
||||
# Wheel positions are ignorable coordinates, so they are not introduced.
|
||||
q1, q2, q4, q5 = dynamicsymbols('q1 q2 q4 q5')
|
||||
q1d, q2d, q4d, q5d = dynamicsymbols('q1 q2 q4 q5', 1)
|
||||
u1, u2, u3, u4, u5, u6 = dynamicsymbols('u1 u2 u3 u4 u5 u6')
|
||||
u1d, u2d, u3d, u4d, u5d, u6d = dynamicsymbols('u1 u2 u3 u4 u5 u6', 1)
|
||||
|
||||
# Declare System's Parameters
|
||||
WFrad, WRrad, htangle, forkoffset = symbols('WFrad WRrad htangle forkoffset')
|
||||
forklength, framelength, forkcg1 = symbols('forklength framelength forkcg1')
|
||||
forkcg3, framecg1, framecg3, Iwr11 = symbols('forkcg3 framecg1 framecg3 Iwr11')
|
||||
Iwr22, Iwf11, Iwf22, Iframe11 = symbols('Iwr22 Iwf11 Iwf22 Iframe11')
|
||||
Iframe22, Iframe33, Iframe31, Ifork11 = symbols('Iframe22 Iframe33 Iframe31 Ifork11')
|
||||
Ifork22, Ifork33, Ifork31, g = symbols('Ifork22 Ifork33 Ifork31 g')
|
||||
mframe, mfork, mwf, mwr = symbols('mframe mfork mwf mwr')
|
||||
|
||||
# Set up reference frames for the system
|
||||
# N - inertial
|
||||
# Y - yaw
|
||||
# R - roll
|
||||
# WR - rear wheel, rotation angle is ignorable coordinate so not oriented
|
||||
# Frame - bicycle frame
|
||||
# TempFrame - statically rotated frame for easier reference inertia definition
|
||||
# Fork - bicycle fork
|
||||
# TempFork - statically rotated frame for easier reference inertia definition
|
||||
# WF - front wheel, again posses a ignorable coordinate
|
||||
N = ReferenceFrame('N')
|
||||
Y = N.orientnew('Y', 'Axis', [q1, N.z])
|
||||
R = Y.orientnew('R', 'Axis', [q2, Y.x])
|
||||
Frame = R.orientnew('Frame', 'Axis', [q4 + htangle, R.y])
|
||||
WR = ReferenceFrame('WR')
|
||||
TempFrame = Frame.orientnew('TempFrame', 'Axis', [-htangle, Frame.y])
|
||||
Fork = Frame.orientnew('Fork', 'Axis', [q5, Frame.x])
|
||||
TempFork = Fork.orientnew('TempFork', 'Axis', [-htangle, Fork.y])
|
||||
WF = ReferenceFrame('WF')
|
||||
|
||||
# Kinematics of the Bicycle First block of code is forming the positions of
|
||||
# the relevant points
|
||||
# rear wheel contact -> rear wheel mass center -> frame mass center +
|
||||
# frame/fork connection -> fork mass center + front wheel mass center ->
|
||||
# front wheel contact point
|
||||
WR_cont = Point('WR_cont')
|
||||
WR_mc = WR_cont.locatenew('WR_mc', WRrad * R.z)
|
||||
Steer = WR_mc.locatenew('Steer', framelength * Frame.z)
|
||||
Frame_mc = WR_mc.locatenew('Frame_mc', - framecg1 * Frame.x
|
||||
+ framecg3 * Frame.z)
|
||||
Fork_mc = Steer.locatenew('Fork_mc', - forkcg1 * Fork.x
|
||||
+ forkcg3 * Fork.z)
|
||||
WF_mc = Steer.locatenew('WF_mc', forklength * Fork.x + forkoffset * Fork.z)
|
||||
WF_cont = WF_mc.locatenew('WF_cont', WFrad * (dot(Fork.y, Y.z) * Fork.y -
|
||||
Y.z).normalize())
|
||||
|
||||
# Set the angular velocity of each frame.
|
||||
# Angular accelerations end up being calculated automatically by
|
||||
# differentiating the angular velocities when first needed.
|
||||
# u1 is yaw rate
|
||||
# u2 is roll rate
|
||||
# u3 is rear wheel rate
|
||||
# u4 is frame pitch rate
|
||||
# u5 is fork steer rate
|
||||
# u6 is front wheel rate
|
||||
Y.set_ang_vel(N, u1 * Y.z)
|
||||
R.set_ang_vel(Y, u2 * R.x)
|
||||
WR.set_ang_vel(Frame, u3 * Frame.y)
|
||||
Frame.set_ang_vel(R, u4 * Frame.y)
|
||||
Fork.set_ang_vel(Frame, u5 * Fork.x)
|
||||
WF.set_ang_vel(Fork, u6 * Fork.y)
|
||||
|
||||
# Form the velocities of the previously defined points, using the 2 - point
|
||||
# theorem (written out by hand here). Accelerations again are calculated
|
||||
# automatically when first needed.
|
||||
WR_cont.set_vel(N, 0)
|
||||
WR_mc.v2pt_theory(WR_cont, N, WR)
|
||||
Steer.v2pt_theory(WR_mc, N, Frame)
|
||||
Frame_mc.v2pt_theory(WR_mc, N, Frame)
|
||||
Fork_mc.v2pt_theory(Steer, N, Fork)
|
||||
WF_mc.v2pt_theory(Steer, N, Fork)
|
||||
WF_cont.v2pt_theory(WF_mc, N, WF)
|
||||
|
||||
# Sets the inertias of each body. Uses the inertia frame to construct the
|
||||
# inertia dyadics. Wheel inertias are only defined by principle moments of
|
||||
# inertia, and are in fact constant in the frame and fork reference frames;
|
||||
# it is for this reason that the orientations of the wheels does not need
|
||||
# to be defined. The frame and fork inertias are defined in the 'Temp'
|
||||
# frames which are fixed to the appropriate body frames; this is to allow
|
||||
# easier input of the reference values of the benchmark paper. Note that
|
||||
# due to slightly different orientations, the products of inertia need to
|
||||
# have their signs flipped; this is done later when entering the numerical
|
||||
# value.
|
||||
|
||||
Frame_I = (inertia(TempFrame, Iframe11, Iframe22, Iframe33, 0, 0, Iframe31), Frame_mc)
|
||||
Fork_I = (inertia(TempFork, Ifork11, Ifork22, Ifork33, 0, 0, Ifork31), Fork_mc)
|
||||
WR_I = (inertia(Frame, Iwr11, Iwr22, Iwr11), WR_mc)
|
||||
WF_I = (inertia(Fork, Iwf11, Iwf22, Iwf11), WF_mc)
|
||||
|
||||
# Declaration of the RigidBody containers. ::
|
||||
|
||||
BodyFrame = RigidBody('BodyFrame', Frame_mc, Frame, mframe, Frame_I)
|
||||
BodyFork = RigidBody('BodyFork', Fork_mc, Fork, mfork, Fork_I)
|
||||
BodyWR = RigidBody('BodyWR', WR_mc, WR, mwr, WR_I)
|
||||
BodyWF = RigidBody('BodyWF', WF_mc, WF, mwf, WF_I)
|
||||
|
||||
# The kinematic differential equations; they are defined quite simply. Each
|
||||
# entry in this list is equal to zero.
|
||||
kd = [q1d - u1, q2d - u2, q4d - u4, q5d - u5]
|
||||
|
||||
# The nonholonomic constraints are the velocity of the front wheel contact
|
||||
# point dotted into the X, Y, and Z directions; the yaw frame is used as it
|
||||
# is "closer" to the front wheel (1 less DCM connecting them). These
|
||||
# constraints force the velocity of the front wheel contact point to be 0
|
||||
# in the inertial frame; the X and Y direction constraints enforce a
|
||||
# "no-slip" condition, and the Z direction constraint forces the front
|
||||
# wheel contact point to not move away from the ground frame, essentially
|
||||
# replicating the holonomic constraint which does not allow the frame pitch
|
||||
# to change in an invalid fashion.
|
||||
|
||||
conlist_speed = [WF_cont.vel(N) & Y.x, WF_cont.vel(N) & Y.y, WF_cont.vel(N) & Y.z]
|
||||
|
||||
# The holonomic constraint is that the position from the rear wheel contact
|
||||
# point to the front wheel contact point when dotted into the
|
||||
# normal-to-ground plane direction must be zero; effectively that the front
|
||||
# and rear wheel contact points are always touching the ground plane. This
|
||||
# is actually not part of the dynamic equations, but instead is necessary
|
||||
# for the lineraization process.
|
||||
|
||||
conlist_coord = [WF_cont.pos_from(WR_cont) & Y.z]
|
||||
|
||||
# The force list; each body has the appropriate gravitational force applied
|
||||
# at its mass center.
|
||||
FL = [(Frame_mc, -mframe * g * Y.z),
|
||||
(Fork_mc, -mfork * g * Y.z),
|
||||
(WF_mc, -mwf * g * Y.z),
|
||||
(WR_mc, -mwr * g * Y.z)]
|
||||
BL = [BodyFrame, BodyFork, BodyWR, BodyWF]
|
||||
|
||||
|
||||
# The N frame is the inertial frame, coordinates are supplied in the order
|
||||
# of independent, dependent coordinates, as are the speeds. The kinematic
|
||||
# differential equation are also entered here. Here the dependent speeds
|
||||
# are specified, in the same order they were provided in earlier, along
|
||||
# with the non-holonomic constraints. The dependent coordinate is also
|
||||
# provided, with the holonomic constraint. Again, this is only provided
|
||||
# for the linearization process.
|
||||
|
||||
KM = KanesMethod(N, q_ind=[q1, q2, q5],
|
||||
q_dependent=[q4], configuration_constraints=conlist_coord,
|
||||
u_ind=[u2, u3, u5],
|
||||
u_dependent=[u1, u4, u6], velocity_constraints=conlist_speed,
|
||||
kd_eqs=kd,
|
||||
constraint_solver="CRAMER")
|
||||
(fr, frstar) = KM.kanes_equations(BL, FL)
|
||||
|
||||
# This is the start of entering in the numerical values from the benchmark
|
||||
# paper to validate the eigen values of the linearized equations from this
|
||||
# model to the reference eigen values. Look at the aforementioned paper for
|
||||
# more information. Some of these are intermediate values, used to
|
||||
# transform values from the paper into the coordinate systems used in this
|
||||
# model.
|
||||
PaperRadRear = 0.3
|
||||
PaperRadFront = 0.35
|
||||
HTA = (pi / 2 - pi / 10).evalf()
|
||||
TrailPaper = 0.08
|
||||
rake = (-(TrailPaper*sin(HTA)-(PaperRadFront*cos(HTA)))).evalf()
|
||||
PaperWb = 1.02
|
||||
PaperFrameCgX = 0.3
|
||||
PaperFrameCgZ = 0.9
|
||||
PaperForkCgX = 0.9
|
||||
PaperForkCgZ = 0.7
|
||||
FrameLength = (PaperWb*sin(HTA)-(rake-(PaperRadFront-PaperRadRear)*cos(HTA))).evalf()
|
||||
FrameCGNorm = ((PaperFrameCgZ - PaperRadRear-(PaperFrameCgX/sin(HTA))*cos(HTA))*sin(HTA)).evalf()
|
||||
FrameCGPar = (PaperFrameCgX / sin(HTA) + (PaperFrameCgZ - PaperRadRear - PaperFrameCgX / sin(HTA) * cos(HTA)) * cos(HTA)).evalf()
|
||||
tempa = (PaperForkCgZ - PaperRadFront)
|
||||
tempb = (PaperWb-PaperForkCgX)
|
||||
tempc = (sqrt(tempa**2+tempb**2)).evalf()
|
||||
PaperForkL = (PaperWb*cos(HTA)-(PaperRadFront-PaperRadRear)*sin(HTA)).evalf()
|
||||
ForkCGNorm = (rake+(tempc * sin(pi/2-HTA-acos(tempa/tempc)))).evalf()
|
||||
ForkCGPar = (tempc * cos((pi/2-HTA)-acos(tempa/tempc))-PaperForkL).evalf()
|
||||
|
||||
# Here is the final assembly of the numerical values. The symbol 'v' is the
|
||||
# forward speed of the bicycle (a concept which only makes sense in the
|
||||
# upright, static equilibrium case?). These are in a dictionary which will
|
||||
# later be substituted in. Again the sign on the *product* of inertia
|
||||
# values is flipped here, due to different orientations of coordinate
|
||||
# systems.
|
||||
v = symbols('v')
|
||||
val_dict = {WFrad: PaperRadFront,
|
||||
WRrad: PaperRadRear,
|
||||
htangle: HTA,
|
||||
forkoffset: rake,
|
||||
forklength: PaperForkL,
|
||||
framelength: FrameLength,
|
||||
forkcg1: ForkCGPar,
|
||||
forkcg3: ForkCGNorm,
|
||||
framecg1: FrameCGNorm,
|
||||
framecg3: FrameCGPar,
|
||||
Iwr11: 0.0603,
|
||||
Iwr22: 0.12,
|
||||
Iwf11: 0.1405,
|
||||
Iwf22: 0.28,
|
||||
Ifork11: 0.05892,
|
||||
Ifork22: 0.06,
|
||||
Ifork33: 0.00708,
|
||||
Ifork31: 0.00756,
|
||||
Iframe11: 9.2,
|
||||
Iframe22: 11,
|
||||
Iframe33: 2.8,
|
||||
Iframe31: -2.4,
|
||||
mfork: 4,
|
||||
mframe: 85,
|
||||
mwf: 3,
|
||||
mwr: 2,
|
||||
g: 9.81,
|
||||
q1: 0,
|
||||
q2: 0,
|
||||
q4: 0,
|
||||
q5: 0,
|
||||
u1: 0,
|
||||
u2: 0,
|
||||
u3: v / PaperRadRear,
|
||||
u4: 0,
|
||||
u5: 0,
|
||||
u6: v / PaperRadFront}
|
||||
|
||||
# Linearizes the forcing vector; the equations are set up as MM udot =
|
||||
# forcing, where MM is the mass matrix, udot is the vector representing the
|
||||
# time derivatives of the generalized speeds, and forcing is a vector which
|
||||
# contains both external forcing terms and internal forcing terms, such as
|
||||
# centripital or coriolis forces. This actually returns a matrix with as
|
||||
# many rows as *total* coordinates and speeds, but only as many columns as
|
||||
# independent coordinates and speeds.
|
||||
|
||||
A, B, _ = KM.linearize(
|
||||
A_and_B=True,
|
||||
op_point={
|
||||
# Operating points for the accelerations are required for the
|
||||
# linearizer to eliminate u' terms showing up in the coefficient
|
||||
# matrices.
|
||||
u1.diff(): 0,
|
||||
u2.diff(): 0,
|
||||
u3.diff(): 0,
|
||||
u4.diff(): 0,
|
||||
u5.diff(): 0,
|
||||
u6.diff(): 0,
|
||||
u1: 0,
|
||||
u2: 0,
|
||||
u3: v / PaperRadRear,
|
||||
u4: 0,
|
||||
u5: 0,
|
||||
u6: v / PaperRadFront,
|
||||
q1: 0,
|
||||
q2: 0,
|
||||
q4: 0,
|
||||
q5: 0,
|
||||
},
|
||||
linear_solver="CRAMER",
|
||||
)
|
||||
# As mentioned above, the size of the linearized forcing terms is expanded
|
||||
# to include both q's and u's, so the mass matrix must have this done as
|
||||
# well. This will likely be changed to be part of the linearized process,
|
||||
# for future reference.
|
||||
A_s = A.xreplace(val_dict)
|
||||
B_s = B.xreplace(val_dict)
|
||||
|
||||
A_s = A_s.evalf()
|
||||
B_s = B_s.evalf()
|
||||
|
||||
# Finally, we construct an "A" matrix for the form xdot = A x (x being the
|
||||
# state vector, although in this case, the sizes are a little off). The
|
||||
# following line extracts only the minimum entries required for eigenvalue
|
||||
# analysis, which correspond to rows and columns for lean, steer, lean
|
||||
# rate, and steer rate.
|
||||
A = A_s.extract([1, 2, 3, 5], [1, 2, 3, 5])
|
||||
|
||||
# Precomputed for comparison
|
||||
Res = Matrix([[ 0, 0, 1.0, 0],
|
||||
[ 0, 0, 0, 1.0],
|
||||
[9.48977444677355, -0.891197738059089*v**2 - 0.571523173729245, -0.105522449805691*v, -0.330515398992311*v],
|
||||
[11.7194768719633, -1.97171508499972*v**2 + 30.9087533932407, 3.67680523332152*v, -3.08486552743311*v]])
|
||||
|
||||
# Actual eigenvalue comparison
|
||||
eps = 1.e-12
|
||||
for i in range(6):
|
||||
error = Res.subs(v, i) - A.subs(v, i)
|
||||
assert all(abs(x) < eps for x in error)
|
||||
+115
@@ -0,0 +1,115 @@
|
||||
from sympy import (cos, sin, Matrix, symbols)
|
||||
from sympy.physics.mechanics import (dynamicsymbols, ReferenceFrame, Point,
|
||||
KanesMethod, Particle)
|
||||
|
||||
def test_replace_qdots_in_force():
|
||||
# Test PR 16700 "Replaces qdots with us in force-list in kanes.py"
|
||||
# The new functionality allows one to specify forces in qdots which will
|
||||
# automatically be replaced with u:s which are defined by the kde supplied
|
||||
# to KanesMethod. The test case is the double pendulum with interacting
|
||||
# forces in the example of chapter 4.7 "CONTRIBUTING INTERACTION FORCES"
|
||||
# in Ref. [1]. Reference list at end test function.
|
||||
|
||||
q1, q2 = dynamicsymbols('q1, q2')
|
||||
qd1, qd2 = dynamicsymbols('q1, q2', level=1)
|
||||
u1, u2 = dynamicsymbols('u1, u2')
|
||||
|
||||
l, m = symbols('l, m')
|
||||
|
||||
N = ReferenceFrame('N') # Inertial frame
|
||||
A = N.orientnew('A', 'Axis', (q1, N.z)) # Rod A frame
|
||||
B = A.orientnew('B', 'Axis', (q2, N.z)) # Rod B frame
|
||||
|
||||
O = Point('O') # Origo
|
||||
O.set_vel(N, 0)
|
||||
|
||||
P = O.locatenew('P', ( l * A.x )) # Point @ end of rod A
|
||||
P.v2pt_theory(O, N, A)
|
||||
|
||||
Q = P.locatenew('Q', ( l * B.x )) # Point @ end of rod B
|
||||
Q.v2pt_theory(P, N, B)
|
||||
|
||||
Ap = Particle('Ap', P, m)
|
||||
Bp = Particle('Bp', Q, m)
|
||||
|
||||
# The forces are specified below. sigma is the torsional spring stiffness
|
||||
# and delta is the viscous damping coefficient acting between the two
|
||||
# bodies. Here, we specify the viscous damper as function of qdots prior
|
||||
# forming the kde. In more complex systems it not might be obvious which
|
||||
# kde is most efficient, why it is convenient to specify viscous forces in
|
||||
# qdots independently of the kde.
|
||||
sig, delta = symbols('sigma, delta')
|
||||
Ta = (sig * q2 + delta * qd2) * N.z
|
||||
forces = [(A, Ta), (B, -Ta)]
|
||||
|
||||
# Try different kdes.
|
||||
kde1 = [u1 - qd1, u2 - qd2]
|
||||
kde2 = [u1 - qd1, u2 - (qd1 + qd2)]
|
||||
|
||||
KM1 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde1)
|
||||
fr1, fstar1 = KM1.kanes_equations([Ap, Bp], forces)
|
||||
|
||||
KM2 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde2)
|
||||
fr2, fstar2 = KM2.kanes_equations([Ap, Bp], forces)
|
||||
|
||||
# Check EOM for KM2:
|
||||
# Mass and force matrix from p.6 in Ref. [2] with added forces from
|
||||
# example of chapter 4.7 in [1] and without gravity.
|
||||
forcing_matrix_expected = Matrix( [ [ m * l**2 * sin(q2) * u2**2 + sig * q2
|
||||
+ delta * (u2 - u1)],
|
||||
[ m * l**2 * sin(q2) * -u1**2 - sig * q2
|
||||
- delta * (u2 - u1)] ] )
|
||||
mass_matrix_expected = Matrix( [ [ 2 * m * l**2, m * l**2 * cos(q2) ],
|
||||
[ m * l**2 * cos(q2), m * l**2 ] ] )
|
||||
|
||||
assert (KM2.mass_matrix.expand() == mass_matrix_expected.expand())
|
||||
assert (KM2.forcing.expand() == forcing_matrix_expected.expand())
|
||||
|
||||
# Check fr1 with reference fr_expected from [1] with u:s instead of qdots.
|
||||
fr1_expected = Matrix([ 0, -(sig*q2 + delta * u2) ])
|
||||
assert fr1.expand() == fr1_expected.expand()
|
||||
|
||||
# Check fr2
|
||||
fr2_expected = Matrix([sig * q2 + delta * (u2 - u1),
|
||||
- sig * q2 - delta * (u2 - u1)])
|
||||
assert fr2.expand() == fr2_expected.expand()
|
||||
|
||||
# Specifying forces in u:s should stay the same:
|
||||
Ta = (sig * q2 + delta * u2) * N.z
|
||||
forces = [(A, Ta), (B, -Ta)]
|
||||
KM1 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde1)
|
||||
fr1, fstar1 = KM1.kanes_equations([Ap, Bp], forces)
|
||||
|
||||
assert fr1.expand() == fr1_expected.expand()
|
||||
|
||||
Ta = (sig * q2 + delta * (u2-u1)) * N.z
|
||||
forces = [(A, Ta), (B, -Ta)]
|
||||
KM2 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde2)
|
||||
fr2, fstar2 = KM2.kanes_equations([Ap, Bp], forces)
|
||||
|
||||
assert fr2.expand() == fr2_expected.expand()
|
||||
|
||||
# Test if we have a qubic qdot force:
|
||||
Ta = (sig * q2 + delta * qd2**3) * N.z
|
||||
forces = [(A, Ta), (B, -Ta)]
|
||||
|
||||
KM1 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde1)
|
||||
fr1, fstar1 = KM1.kanes_equations([Ap, Bp], forces)
|
||||
|
||||
fr1_cubic_expected = Matrix([ 0, -(sig*q2 + delta * u2**3) ])
|
||||
|
||||
assert fr1.expand() == fr1_cubic_expected.expand()
|
||||
|
||||
KM2 = KanesMethod(N, [q1, q2], [u1, u2], kd_eqs=kde2)
|
||||
fr2, fstar2 = KM2.kanes_equations([Ap, Bp], forces)
|
||||
|
||||
fr2_cubic_expected = Matrix([sig * q2 + delta * (u2 - u1)**3,
|
||||
- sig * q2 - delta * (u2 - u1)**3])
|
||||
|
||||
assert fr2.expand() == fr2_cubic_expected.expand()
|
||||
|
||||
# References:
|
||||
# [1] T.R. Kane, D. a Levinson, Dynamics Theory and Applications, 2005.
|
||||
# [2] Arun K Banerjee, Flexible Multibody Dynamics:Efficient Formulations
|
||||
# and Applications, John Wiley and Sons, Ltd, 2016.
|
||||
# doi:http://dx.doi.org/10.1002/9781119015635.
|
||||
+128
@@ -0,0 +1,128 @@
|
||||
from sympy import (zeros, Matrix, symbols, lambdify, sqrt, pi,
|
||||
simplify)
|
||||
from sympy.physics.mechanics import (dynamicsymbols, cross, inertia, RigidBody,
|
||||
ReferenceFrame, KanesMethod)
|
||||
|
||||
|
||||
def _create_rolling_disc():
|
||||
# Define symbols and coordinates
|
||||
t = dynamicsymbols._t
|
||||
q1, q2, q3, q4, q5, u1, u2, u3, u4, u5 = dynamicsymbols('q1:6 u1:6')
|
||||
g, r, m = symbols('g r m')
|
||||
# Define bodies and frames
|
||||
ground = RigidBody('ground')
|
||||
disc = RigidBody('disk', mass=m)
|
||||
disc.inertia = (m * r ** 2 / 4 * inertia(disc.frame, 1, 2, 1),
|
||||
disc.masscenter)
|
||||
ground.masscenter.set_vel(ground.frame, 0)
|
||||
disc.masscenter.set_vel(disc.frame, 0)
|
||||
int_frame = ReferenceFrame('int_frame')
|
||||
# Orient frames
|
||||
int_frame.orient_body_fixed(ground.frame, (q1, q2, 0), 'zxy')
|
||||
disc.frame.orient_axis(int_frame, int_frame.y, q3)
|
||||
g_w_d = disc.frame.ang_vel_in(ground.frame)
|
||||
disc.frame.set_ang_vel(ground.frame,
|
||||
u1 * disc.x + u2 * disc.y + u3 * disc.z)
|
||||
# Define points
|
||||
cp = ground.masscenter.locatenew('contact_point',
|
||||
q4 * ground.x + q5 * ground.y)
|
||||
cp.set_vel(ground.frame, u4 * ground.x + u5 * ground.y)
|
||||
disc.masscenter.set_pos(cp, r * int_frame.z)
|
||||
disc.masscenter.set_vel(ground.frame, cross(
|
||||
disc.frame.ang_vel_in(ground.frame), disc.masscenter.pos_from(cp)))
|
||||
# Define kinematic differential equations
|
||||
kdes = [g_w_d.dot(disc.x) - u1, g_w_d.dot(disc.y) - u2,
|
||||
g_w_d.dot(disc.z) - u3, q4.diff(t) - u4, q5.diff(t) - u5]
|
||||
# Define nonholonomic constraints
|
||||
v0 = cp.vel(ground.frame) + cross(
|
||||
disc.frame.ang_vel_in(int_frame), cp.pos_from(disc.masscenter))
|
||||
fnh = [v0.dot(ground.x), v0.dot(ground.y)]
|
||||
# Define loads
|
||||
loads = [(disc.masscenter, -disc.mass * g * ground.z)]
|
||||
bodies = [disc]
|
||||
return {
|
||||
'frame': ground.frame,
|
||||
'q_ind': [q1, q2, q3, q4, q5],
|
||||
'u_ind': [u1, u2, u3],
|
||||
'u_dep': [u4, u5],
|
||||
'kdes': kdes,
|
||||
'fnh': fnh,
|
||||
'bodies': bodies,
|
||||
'loads': loads
|
||||
}
|
||||
|
||||
|
||||
def _verify_rolling_disc_numerically(kane, all_zero=False):
|
||||
q, u, p = dynamicsymbols('q1:6'), dynamicsymbols('u1:6'), symbols('g r m')
|
||||
eval_sys = lambdify((q, u, p), (kane.mass_matrix_full, kane.forcing_full),
|
||||
cse=True)
|
||||
solve_sys = lambda q, u, p: Matrix.LUsolve(
|
||||
*(Matrix(mat) for mat in eval_sys(q, u, p)))
|
||||
solve_u_dep = lambdify((q, u[:3], p), kane._Ars * Matrix(u[:3]), cse=True)
|
||||
eps = 1e-10
|
||||
p_vals = (9.81, 0.26, 3.43)
|
||||
# First numeric test
|
||||
q_vals = (0.3, 0.1, 1.97, -0.35, 2.27)
|
||||
u_vals = [-0.2, 1.3, 0.15]
|
||||
u_vals.extend(solve_u_dep(q_vals, u_vals, p_vals)[:2, 0])
|
||||
expected = Matrix([
|
||||
0.126603940595934, 0.215942571601660, 1.28736069604936,
|
||||
0.319764288376543, 0.0989146857254898, -0.925848952664489,
|
||||
-0.0181350656532944, 2.91695398184589, -0.00992793421754526,
|
||||
0.0412861634829171])
|
||||
assert all(abs(x) < eps for x in
|
||||
(solve_sys(q_vals, u_vals, p_vals) - expected))
|
||||
# Second numeric test
|
||||
q_vals = (3.97, -0.28, 8.2, -0.35, 2.27)
|
||||
u_vals = [-0.25, -2.2, 0.62]
|
||||
u_vals.extend(solve_u_dep(q_vals, u_vals, p_vals)[:2, 0])
|
||||
expected = Matrix([
|
||||
0.0259159090798597, 0.668041660387416, -2.19283799213811,
|
||||
0.385441810852219, 0.420109283790573, 1.45030568179066,
|
||||
-0.0110924422400793, -8.35617840186040, -0.154098542632173,
|
||||
-0.146102664410010])
|
||||
assert all(abs(x) < eps for x in
|
||||
(solve_sys(q_vals, u_vals, p_vals) - expected))
|
||||
if all_zero:
|
||||
q_vals = (0, 0, 0, 0, 0)
|
||||
u_vals = (0, 0, 0, 0, 0)
|
||||
assert solve_sys(q_vals, u_vals, p_vals) == zeros(10, 1)
|
||||
|
||||
|
||||
def test_kane_rolling_disc_lu():
|
||||
props = _create_rolling_disc()
|
||||
kane = KanesMethod(props['frame'], props['q_ind'], props['u_ind'],
|
||||
props['kdes'], u_dependent=props['u_dep'],
|
||||
velocity_constraints=props['fnh'],
|
||||
bodies=props['bodies'], forcelist=props['loads'],
|
||||
explicit_kinematics=False, constraint_solver='LU')
|
||||
kane.kanes_equations()
|
||||
_verify_rolling_disc_numerically(kane)
|
||||
|
||||
|
||||
def test_kane_rolling_disc_kdes_callable():
|
||||
props = _create_rolling_disc()
|
||||
kane = KanesMethod(
|
||||
props['frame'], props['q_ind'], props['u_ind'], props['kdes'],
|
||||
u_dependent=props['u_dep'], velocity_constraints=props['fnh'],
|
||||
bodies=props['bodies'], forcelist=props['loads'],
|
||||
explicit_kinematics=False,
|
||||
kd_eqs_solver=lambda A, b: simplify(A.LUsolve(b)))
|
||||
q, u, p = dynamicsymbols('q1:6'), dynamicsymbols('u1:6'), symbols('g r m')
|
||||
qd = dynamicsymbols('q1:6', 1)
|
||||
eval_kdes = lambdify((q, qd, u, p), tuple(kane.kindiffdict().items()))
|
||||
eps = 1e-10
|
||||
# Test with only zeros. If 'LU' would be used this would result in nan.
|
||||
p_vals = (9.81, 0.25, 3.5)
|
||||
zero_vals = (0, 0, 0, 0, 0)
|
||||
assert all(abs(qdi - fui) < eps for qdi, fui in
|
||||
eval_kdes(zero_vals, zero_vals, zero_vals, p_vals))
|
||||
# Test with some arbitrary values
|
||||
q_vals = tuple(map(float, (pi / 6, pi / 3, pi / 2, 0.42, 0.62)))
|
||||
qd_vals = tuple(map(float, (4, 1 / 3, 4 - 2 * sqrt(3),
|
||||
0.25 * (2 * sqrt(3) - 3),
|
||||
0.25 * (2 - sqrt(3)))))
|
||||
u_vals = tuple(map(float, (-2, 4, 1 / 3, 0.25 * (-3 + 2 * sqrt(3)),
|
||||
0.25 * (-sqrt(3) + 2))))
|
||||
assert all(abs(qdi - fui) < eps for qdi, fui in
|
||||
eval_kdes(q_vals, qd_vals, u_vals, p_vals))
|
||||
+247
@@ -0,0 +1,247 @@
|
||||
from sympy.physics.mechanics import (dynamicsymbols, ReferenceFrame, Point,
|
||||
RigidBody, LagrangesMethod, Particle,
|
||||
inertia, Lagrangian)
|
||||
from sympy.core.function import (Derivative, Function)
|
||||
from sympy.core.numbers import pi
|
||||
from sympy.core.symbol import symbols
|
||||
from sympy.functions.elementary.trigonometric import (cos, sin, tan)
|
||||
from sympy.matrices.dense import Matrix
|
||||
from sympy.simplify.simplify import simplify
|
||||
from sympy.testing.pytest import raises
|
||||
|
||||
|
||||
def test_invalid_coordinates():
|
||||
# Simple pendulum, but use symbol instead of dynamicsymbol
|
||||
l, m, g = symbols('l m g')
|
||||
q = symbols('q') # Generalized coordinate
|
||||
N, O = ReferenceFrame('N'), Point('O')
|
||||
O.set_vel(N, 0)
|
||||
P = Particle('P', Point('P'), m)
|
||||
P.point.set_pos(O, l * (sin(q) * N.x - cos(q) * N.y))
|
||||
P.potential_energy = m * g * P.point.pos_from(O).dot(N.y)
|
||||
L = Lagrangian(N, P)
|
||||
raises(ValueError, lambda: LagrangesMethod(L, [q], bodies=P))
|
||||
|
||||
|
||||
def test_disc_on_an_incline_plane():
|
||||
# Disc rolling on an inclined plane
|
||||
# First the generalized coordinates are created. The mass center of the
|
||||
# disc is located from top vertex of the inclined plane by the generalized
|
||||
# coordinate 'y'. The orientation of the disc is defined by the angle
|
||||
# 'theta'. The mass of the disc is 'm' and its radius is 'R'. The length of
|
||||
# the inclined path is 'l', the angle of inclination is 'alpha'. 'g' is the
|
||||
# gravitational constant.
|
||||
y, theta = dynamicsymbols('y theta')
|
||||
yd, thetad = dynamicsymbols('y theta', 1)
|
||||
m, g, R, l, alpha = symbols('m g R l alpha')
|
||||
|
||||
# Next, we create the inertial reference frame 'N'. A reference frame 'A'
|
||||
# is attached to the inclined plane. Finally a frame is created which is attached to the disk.
|
||||
N = ReferenceFrame('N')
|
||||
A = N.orientnew('A', 'Axis', [pi/2 - alpha, N.z])
|
||||
B = A.orientnew('B', 'Axis', [-theta, A.z])
|
||||
|
||||
# Creating the disc 'D'; we create the point that represents the mass
|
||||
# center of the disc and set its velocity. The inertia dyadic of the disc
|
||||
# is created. Finally, we create the disc.
|
||||
Do = Point('Do')
|
||||
Do.set_vel(N, yd * A.x)
|
||||
I = m * R**2/2 * B.z | B.z
|
||||
D = RigidBody('D', Do, B, m, (I, Do))
|
||||
|
||||
# To construct the Lagrangian, 'L', of the disc, we determine its kinetic
|
||||
# and potential energies, T and U, respectively. L is defined as the
|
||||
# difference between T and U.
|
||||
D.potential_energy = m * g * (l - y) * sin(alpha)
|
||||
L = Lagrangian(N, D)
|
||||
|
||||
# We then create the list of generalized coordinates and constraint
|
||||
# equations. The constraint arises due to the disc rolling without slip on
|
||||
# on the inclined path. We then invoke the 'LagrangesMethod' class and
|
||||
# supply it the necessary arguments and generate the equations of motion.
|
||||
# The'rhs' method solves for the q_double_dots (i.e. the second derivative
|
||||
# with respect to time of the generalized coordinates and the lagrange
|
||||
# multipliers.
|
||||
q = [y, theta]
|
||||
hol_coneqs = [y - R * theta]
|
||||
m = LagrangesMethod(L, q, hol_coneqs=hol_coneqs)
|
||||
m.form_lagranges_equations()
|
||||
rhs = m.rhs()
|
||||
rhs.simplify()
|
||||
assert rhs[2] == 2*g*sin(alpha)/3
|
||||
|
||||
|
||||
def test_simp_pen():
|
||||
# This tests that the equations generated by LagrangesMethod are identical
|
||||
# to those obtained by hand calculations. The system under consideration is
|
||||
# the simple pendulum.
|
||||
# We begin by creating the generalized coordinates as per the requirements
|
||||
# of LagrangesMethod. Also we created the associate symbols
|
||||
# that characterize the system: 'm' is the mass of the bob, l is the length
|
||||
# of the massless rigid rod connecting the bob to a point O fixed in the
|
||||
# inertial frame.
|
||||
q, u = dynamicsymbols('q u')
|
||||
qd, ud = dynamicsymbols('q u ', 1)
|
||||
l, m, g = symbols('l m g')
|
||||
|
||||
# We then create the inertial frame and a frame attached to the massless
|
||||
# string following which we define the inertial angular velocity of the
|
||||
# string.
|
||||
N = ReferenceFrame('N')
|
||||
A = N.orientnew('A', 'Axis', [q, N.z])
|
||||
A.set_ang_vel(N, qd * N.z)
|
||||
|
||||
# Next, we create the point O and fix it in the inertial frame. We then
|
||||
# locate the point P to which the bob is attached. Its corresponding
|
||||
# velocity is then determined by the 'two point formula'.
|
||||
O = Point('O')
|
||||
O.set_vel(N, 0)
|
||||
P = O.locatenew('P', l * A.x)
|
||||
P.v2pt_theory(O, N, A)
|
||||
|
||||
# The 'Particle' which represents the bob is then created and its
|
||||
# Lagrangian generated.
|
||||
Pa = Particle('Pa', P, m)
|
||||
Pa.potential_energy = - m * g * l * cos(q)
|
||||
L = Lagrangian(N, Pa)
|
||||
|
||||
# The 'LagrangesMethod' class is invoked to obtain equations of motion.
|
||||
lm = LagrangesMethod(L, [q])
|
||||
lm.form_lagranges_equations()
|
||||
RHS = lm.rhs()
|
||||
assert RHS[1] == -g*sin(q)/l
|
||||
|
||||
|
||||
def test_nonminimal_pendulum():
|
||||
q1, q2 = dynamicsymbols('q1:3')
|
||||
q1d, q2d = dynamicsymbols('q1:3', level=1)
|
||||
L, m, t = symbols('L, m, t')
|
||||
g = 9.8
|
||||
# Compose World Frame
|
||||
N = ReferenceFrame('N')
|
||||
pN = Point('N*')
|
||||
pN.set_vel(N, 0)
|
||||
# Create point P, the pendulum mass
|
||||
P = pN.locatenew('P1', q1*N.x + q2*N.y)
|
||||
P.set_vel(N, P.pos_from(pN).dt(N))
|
||||
pP = Particle('pP', P, m)
|
||||
# Constraint Equations
|
||||
f_c = Matrix([q1**2 + q2**2 - L**2])
|
||||
# Calculate the lagrangian, and form the equations of motion
|
||||
Lag = Lagrangian(N, pP)
|
||||
LM = LagrangesMethod(Lag, [q1, q2], hol_coneqs=f_c,
|
||||
forcelist=[(P, m*g*N.x)], frame=N)
|
||||
LM.form_lagranges_equations()
|
||||
# Check solution
|
||||
lam1 = LM.lam_vec[0, 0]
|
||||
eom_sol = Matrix([[m*Derivative(q1, t, t) - 9.8*m + 2*lam1*q1],
|
||||
[m*Derivative(q2, t, t) + 2*lam1*q2]])
|
||||
assert LM.eom == eom_sol
|
||||
# Check multiplier solution
|
||||
lam_sol = Matrix([(19.6*q1 + 2*q1d**2 + 2*q2d**2)/(4*q1**2/m + 4*q2**2/m)])
|
||||
assert simplify(LM.solve_multipliers(sol_type='Matrix')) == simplify(lam_sol)
|
||||
|
||||
|
||||
def test_dub_pen():
|
||||
|
||||
# The system considered is the double pendulum. Like in the
|
||||
# test of the simple pendulum above, we begin by creating the generalized
|
||||
# coordinates and the simple generalized speeds and accelerations which
|
||||
# will be used later. Following this we create frames and points necessary
|
||||
# for the kinematics. The procedure isn't explicitly explained as this is
|
||||
# similar to the simple pendulum. Also this is documented on the pydy.org
|
||||
# website.
|
||||
q1, q2 = dynamicsymbols('q1 q2')
|
||||
q1d, q2d = dynamicsymbols('q1 q2', 1)
|
||||
q1dd, q2dd = dynamicsymbols('q1 q2', 2)
|
||||
u1, u2 = dynamicsymbols('u1 u2')
|
||||
u1d, u2d = dynamicsymbols('u1 u2', 1)
|
||||
l, m, g = symbols('l m g')
|
||||
|
||||
N = ReferenceFrame('N')
|
||||
A = N.orientnew('A', 'Axis', [q1, N.z])
|
||||
B = N.orientnew('B', 'Axis', [q2, N.z])
|
||||
|
||||
A.set_ang_vel(N, q1d * A.z)
|
||||
B.set_ang_vel(N, q2d * A.z)
|
||||
|
||||
O = Point('O')
|
||||
P = O.locatenew('P', l * A.x)
|
||||
R = P.locatenew('R', l * B.x)
|
||||
|
||||
O.set_vel(N, 0)
|
||||
P.v2pt_theory(O, N, A)
|
||||
R.v2pt_theory(P, N, B)
|
||||
|
||||
ParP = Particle('ParP', P, m)
|
||||
ParR = Particle('ParR', R, m)
|
||||
|
||||
ParP.potential_energy = - m * g * l * cos(q1)
|
||||
ParR.potential_energy = - m * g * l * cos(q1) - m * g * l * cos(q2)
|
||||
L = Lagrangian(N, ParP, ParR)
|
||||
lm = LagrangesMethod(L, [q1, q2], bodies=[ParP, ParR])
|
||||
lm.form_lagranges_equations()
|
||||
|
||||
assert simplify(l*m*(2*g*sin(q1) + l*sin(q1)*sin(q2)*q2dd
|
||||
+ l*sin(q1)*cos(q2)*q2d**2 - l*sin(q2)*cos(q1)*q2d**2
|
||||
+ l*cos(q1)*cos(q2)*q2dd + 2*l*q1dd) - lm.eom[0]) == 0
|
||||
assert simplify(l*m*(g*sin(q2) + l*sin(q1)*sin(q2)*q1dd
|
||||
- l*sin(q1)*cos(q2)*q1d**2 + l*sin(q2)*cos(q1)*q1d**2
|
||||
+ l*cos(q1)*cos(q2)*q1dd + l*q2dd) - lm.eom[1]) == 0
|
||||
assert lm.bodies == [ParP, ParR]
|
||||
|
||||
|
||||
def test_rolling_disc():
|
||||
# Rolling Disc Example
|
||||
# Here the rolling disc is formed from the contact point up, removing the
|
||||
# need to introduce generalized speeds. Only 3 configuration and 3
|
||||
# speed variables are need to describe this system, along with the
|
||||
# disc's mass and radius, and the local gravity.
|
||||
q1, q2, q3 = dynamicsymbols('q1 q2 q3')
|
||||
q1d, q2d, q3d = dynamicsymbols('q1 q2 q3', 1)
|
||||
r, m, g = symbols('r m g')
|
||||
|
||||
# The kinematics are formed by a series of simple rotations. Each simple
|
||||
# rotation creates a new frame, and the next rotation is defined by the new
|
||||
# frame's basis vectors. This example uses a 3-1-2 series of rotations, or
|
||||
# Z, X, Y series of rotations. Angular velocity for this is defined using
|
||||
# the second frame's basis (the lean frame).
|
||||
N = ReferenceFrame('N')
|
||||
Y = N.orientnew('Y', 'Axis', [q1, N.z])
|
||||
L = Y.orientnew('L', 'Axis', [q2, Y.x])
|
||||
R = L.orientnew('R', 'Axis', [q3, L.y])
|
||||
|
||||
# This is the translational kinematics. We create a point with no velocity
|
||||
# in N; this is the contact point between the disc and ground. Next we form
|
||||
# the position vector from the contact point to the disc's center of mass.
|
||||
# Finally we form the velocity and acceleration of the disc.
|
||||
C = Point('C')
|
||||
C.set_vel(N, 0)
|
||||
Dmc = C.locatenew('Dmc', r * L.z)
|
||||
Dmc.v2pt_theory(C, N, R)
|
||||
|
||||
# Forming the inertia dyadic.
|
||||
I = inertia(L, m/4 * r**2, m/2 * r**2, m/4 * r**2)
|
||||
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
|
||||
|
||||
# Finally we form the equations of motion, using the same steps we did
|
||||
# before. Supply the Lagrangian, the generalized speeds.
|
||||
BodyD.potential_energy = - m * g * r * cos(q2)
|
||||
Lag = Lagrangian(N, BodyD)
|
||||
q = [q1, q2, q3]
|
||||
q1 = Function('q1')
|
||||
q2 = Function('q2')
|
||||
q3 = Function('q3')
|
||||
l = LagrangesMethod(Lag, q)
|
||||
l.form_lagranges_equations()
|
||||
RHS = l.rhs()
|
||||
RHS.simplify()
|
||||
t = symbols('t')
|
||||
|
||||
assert (l.mass_matrix[3:6] == [0, 5*m*r**2/4, 0])
|
||||
assert RHS[4].simplify() == (
|
||||
(-8*g*sin(q2(t)) + r*(5*sin(2*q2(t))*Derivative(q1(t), t) +
|
||||
12*cos(q2(t))*Derivative(q3(t), t))*Derivative(q1(t), t))/(10*r))
|
||||
assert RHS[5] == (-5*cos(q2(t))*Derivative(q1(t), t) + 6*tan(q2(t)
|
||||
)*Derivative(q3(t), t) + 4*Derivative(q1(t), t)/cos(q2(t))
|
||||
)*Derivative(q2(t), t)
|
||||
+46
@@ -0,0 +1,46 @@
|
||||
from sympy import symbols
|
||||
from sympy.physics.mechanics import dynamicsymbols
|
||||
from sympy.physics.mechanics import ReferenceFrame, Point, Particle
|
||||
from sympy.physics.mechanics import LagrangesMethod, Lagrangian
|
||||
|
||||
### This test asserts that a system with more than one external forces
|
||||
### is accurately formed with Lagrange method (see issue #8626)
|
||||
|
||||
def test_lagrange_2forces():
|
||||
### Equations for two damped springs in series with two forces
|
||||
|
||||
### generalized coordinates
|
||||
q1, q2 = dynamicsymbols('q1, q2')
|
||||
### generalized speeds
|
||||
q1d, q2d = dynamicsymbols('q1, q2', 1)
|
||||
|
||||
### Mass, spring strength, friction coefficient
|
||||
m, k, nu = symbols('m, k, nu')
|
||||
|
||||
N = ReferenceFrame('N')
|
||||
O = Point('O')
|
||||
|
||||
### Two points
|
||||
P1 = O.locatenew('P1', q1 * N.x)
|
||||
P1.set_vel(N, q1d * N.x)
|
||||
P2 = O.locatenew('P1', q2 * N.x)
|
||||
P2.set_vel(N, q2d * N.x)
|
||||
|
||||
pP1 = Particle('pP1', P1, m)
|
||||
pP1.potential_energy = k * q1**2 / 2
|
||||
|
||||
pP2 = Particle('pP2', P2, m)
|
||||
pP2.potential_energy = k * (q1 - q2)**2 / 2
|
||||
|
||||
#### Friction forces
|
||||
forcelist = [(P1, - nu * q1d * N.x),
|
||||
(P2, - nu * q2d * N.x)]
|
||||
lag = Lagrangian(N, pP1, pP2)
|
||||
|
||||
l_method = LagrangesMethod(lag, (q1, q2), forcelist=forcelist, frame=N)
|
||||
l_method.form_lagranges_equations()
|
||||
|
||||
eq1 = l_method.eom[0]
|
||||
assert eq1.diff(q1d) == nu
|
||||
eq2 = l_method.eom[1]
|
||||
assert eq2.diff(q2d) == nu
|
||||
+41
@@ -0,0 +1,41 @@
|
||||
from sympy import symbols, sin, cos
|
||||
from sympy.physics.mechanics import (dynamicsymbols, ReferenceFrame, Point,
|
||||
KanesMethod)
|
||||
from sympy.testing import pytest
|
||||
from sympy.solvers.solveset import NonlinearError
|
||||
|
||||
def test_linearity_of_motion_constraints():
|
||||
# Test that an error is raised by KanesMethod if nonlinear velocity
|
||||
# constraints are supplied.
|
||||
# It is a simple pendulum.
|
||||
t = dynamicsymbols._t
|
||||
N, A = ReferenceFrame('N'), ReferenceFrame('A')
|
||||
O, P = Point('O'), Point('P')
|
||||
O.set_vel(N, 0)
|
||||
|
||||
l = symbols('l')
|
||||
q, x, y, u, ux, uy = dynamicsymbols('q x y u ux uy')
|
||||
|
||||
A.orient_axis(N, q, N.z)
|
||||
A.set_ang_vel(N, u * N.z)
|
||||
P.set_pos(O, -l * A.y)
|
||||
P.v2pt_theory(O, N, A)
|
||||
|
||||
kd = [u - q.diff(t), ux - x.diff(t), uy - y.diff(t)]
|
||||
config_constr = [x - l * sin(q), y - l * cos(q)]
|
||||
|
||||
q_ind = [q]
|
||||
q_dep = [x, y]
|
||||
u_ind = [u]
|
||||
u_dep = [ux, uy]
|
||||
|
||||
# Make sure an error is raised if nonlinear velocity constraints are
|
||||
# supplied.
|
||||
speed_constr = [ux - l * q.diff(t) * cos(q), sin(uy) +
|
||||
l * q.diff(t) * sin(q)]
|
||||
|
||||
with pytest.raises(NonlinearError):
|
||||
KanesMethod(N, q_ind=q_ind, q_dependent=q_dep, u_ind=u_ind,
|
||||
u_dependent=u_dep, kd_eqs=kd,
|
||||
configuration_constraints=config_constr,
|
||||
velocity_constraints=speed_constr)
|
||||
+372
@@ -0,0 +1,372 @@
|
||||
from sympy import symbols, Matrix, cos, sin, atan, sqrt, Rational
|
||||
from sympy.core.sympify import sympify
|
||||
from sympy.simplify.simplify import simplify
|
||||
from sympy.solvers.solvers import solve
|
||||
from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame, Point,\
|
||||
dot, cross, inertia, KanesMethod, Particle, RigidBody, Lagrangian,\
|
||||
LagrangesMethod
|
||||
from sympy.testing.pytest import slow
|
||||
|
||||
|
||||
@slow
|
||||
def test_linearize_rolling_disc_kane():
|
||||
# Symbols for time and constant parameters
|
||||
t, r, m, g, v = symbols('t r m g v')
|
||||
|
||||
# Configuration variables and their time derivatives
|
||||
q1, q2, q3, q4, q5, q6 = q = dynamicsymbols('q1:7')
|
||||
q1d, q2d, q3d, q4d, q5d, q6d = qd = [qi.diff(t) for qi in q]
|
||||
|
||||
# Generalized speeds and their time derivatives
|
||||
u = dynamicsymbols('u:6')
|
||||
u1, u2, u3, u4, u5, u6 = u = dynamicsymbols('u1:7')
|
||||
u1d, u2d, u3d, u4d, u5d, u6d = [ui.diff(t) for ui in u]
|
||||
|
||||
# Reference frames
|
||||
N = ReferenceFrame('N') # Inertial frame
|
||||
NO = Point('NO') # Inertial origin
|
||||
A = N.orientnew('A', 'Axis', [q1, N.z]) # Yaw intermediate frame
|
||||
B = A.orientnew('B', 'Axis', [q2, A.x]) # Lean intermediate frame
|
||||
C = B.orientnew('C', 'Axis', [q3, B.y]) # Disc fixed frame
|
||||
CO = NO.locatenew('CO', q4*N.x + q5*N.y + q6*N.z) # Disc center
|
||||
|
||||
# Disc angular velocity in N expressed using time derivatives of coordinates
|
||||
w_c_n_qd = C.ang_vel_in(N)
|
||||
w_b_n_qd = B.ang_vel_in(N)
|
||||
|
||||
# Inertial angular velocity and angular acceleration of disc fixed frame
|
||||
C.set_ang_vel(N, u1*B.x + u2*B.y + u3*B.z)
|
||||
|
||||
# Disc center velocity in N expressed using time derivatives of coordinates
|
||||
v_co_n_qd = CO.pos_from(NO).dt(N)
|
||||
|
||||
# Disc center velocity in N expressed using generalized speeds
|
||||
CO.set_vel(N, u4*C.x + u5*C.y + u6*C.z)
|
||||
|
||||
# Disc Ground Contact Point
|
||||
P = CO.locatenew('P', r*B.z)
|
||||
P.v2pt_theory(CO, N, C)
|
||||
|
||||
# Configuration constraint
|
||||
f_c = Matrix([q6 - dot(CO.pos_from(P), N.z)])
|
||||
|
||||
# Velocity level constraints
|
||||
f_v = Matrix([dot(P.vel(N), uv) for uv in C])
|
||||
|
||||
# Kinematic differential equations
|
||||
kindiffs = Matrix([dot(w_c_n_qd - C.ang_vel_in(N), uv) for uv in B] +
|
||||
[dot(v_co_n_qd - CO.vel(N), uv) for uv in N])
|
||||
qdots = solve(kindiffs, qd)
|
||||
|
||||
# Set angular velocity of remaining frames
|
||||
B.set_ang_vel(N, w_b_n_qd.subs(qdots))
|
||||
C.set_ang_acc(N, C.ang_vel_in(N).dt(B) + cross(B.ang_vel_in(N), C.ang_vel_in(N)))
|
||||
|
||||
# Active forces
|
||||
F_CO = m*g*A.z
|
||||
|
||||
# Create inertia dyadic of disc C about point CO
|
||||
I = (m * r**2) / 4
|
||||
J = (m * r**2) / 2
|
||||
I_C_CO = inertia(C, I, J, I)
|
||||
|
||||
Disc = RigidBody('Disc', CO, C, m, (I_C_CO, CO))
|
||||
BL = [Disc]
|
||||
FL = [(CO, F_CO)]
|
||||
KM = KanesMethod(N, [q1, q2, q3, q4, q5], [u1, u2, u3], kd_eqs=kindiffs,
|
||||
q_dependent=[q6], configuration_constraints=f_c,
|
||||
u_dependent=[u4, u5, u6], velocity_constraints=f_v)
|
||||
(fr, fr_star) = KM.kanes_equations(BL, FL)
|
||||
|
||||
# Test generalized form equations
|
||||
linearizer = KM.to_linearizer()
|
||||
assert linearizer.f_c == f_c
|
||||
assert linearizer.f_v == f_v
|
||||
assert linearizer.f_a == f_v.diff(t).subs(KM.kindiffdict())
|
||||
sol = solve(linearizer.f_0 + linearizer.f_1, qd)
|
||||
for qi in qdots.keys():
|
||||
assert sol[qi] == qdots[qi]
|
||||
assert simplify(linearizer.f_2 + linearizer.f_3 - fr - fr_star) == Matrix([0, 0, 0])
|
||||
|
||||
# Perform the linearization
|
||||
# Precomputed operating point
|
||||
q_op = {q6: -r*cos(q2)}
|
||||
u_op = {u1: 0,
|
||||
u2: sin(q2)*q1d + q3d,
|
||||
u3: cos(q2)*q1d,
|
||||
u4: -r*(sin(q2)*q1d + q3d)*cos(q3),
|
||||
u5: 0,
|
||||
u6: -r*(sin(q2)*q1d + q3d)*sin(q3)}
|
||||
qd_op = {q2d: 0,
|
||||
q4d: -r*(sin(q2)*q1d + q3d)*cos(q1),
|
||||
q5d: -r*(sin(q2)*q1d + q3d)*sin(q1),
|
||||
q6d: 0}
|
||||
ud_op = {u1d: 4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5,
|
||||
u2d: 0,
|
||||
u3d: 0,
|
||||
u4d: r*(sin(q2)*sin(q3)*q1d*q3d + sin(q3)*q3d**2),
|
||||
u5d: r*(4*g*sin(q2)/(5*r) + sin(2*q2)*q1d**2/2 + 6*cos(q2)*q1d*q3d/5),
|
||||
u6d: -r*(sin(q2)*cos(q3)*q1d*q3d + cos(q3)*q3d**2)}
|
||||
|
||||
A, B = linearizer.linearize(op_point=[q_op, u_op, qd_op, ud_op], A_and_B=True, simplify=True)
|
||||
|
||||
upright_nominal = {q1d: 0, q2: 0, m: 1, r: 1, g: 1}
|
||||
|
||||
# Precomputed solution
|
||||
A_sol = Matrix([[0, 0, 0, 0, 0, 0, 0, 1],
|
||||
[0, 0, 0, 0, 0, 1, 0, 0],
|
||||
[0, 0, 0, 0, 0, 0, 1, 0],
|
||||
[sin(q1)*q3d, 0, 0, 0, 0, -sin(q1), -cos(q1), 0],
|
||||
[-cos(q1)*q3d, 0, 0, 0, 0, cos(q1), -sin(q1), 0],
|
||||
[0, Rational(4, 5), 0, 0, 0, 0, 0, 6*q3d/5],
|
||||
[0, 0, 0, 0, 0, 0, 0, 0],
|
||||
[0, 0, 0, 0, 0, -2*q3d, 0, 0]])
|
||||
B_sol = Matrix([])
|
||||
|
||||
# Check that linearization is correct
|
||||
assert A.subs(upright_nominal) == A_sol
|
||||
assert B.subs(upright_nominal) == B_sol
|
||||
|
||||
# Check eigenvalues at critical speed are all zero:
|
||||
assert sympify(A.subs(upright_nominal).subs(q3d, 1/sqrt(3))).eigenvals() == {0: 8}
|
||||
|
||||
# Check whether alternative solvers work
|
||||
# symengine doesn't support method='GJ'
|
||||
linearizer = KM.to_linearizer(linear_solver='GJ')
|
||||
A, B = linearizer.linearize(op_point=[q_op, u_op, qd_op, ud_op],
|
||||
A_and_B=True, simplify=True)
|
||||
assert A.subs(upright_nominal) == A_sol
|
||||
assert B.subs(upright_nominal) == B_sol
|
||||
|
||||
def test_linearize_pendulum_kane_minimal():
|
||||
q1 = dynamicsymbols('q1') # angle of pendulum
|
||||
u1 = dynamicsymbols('u1') # Angular velocity
|
||||
q1d = dynamicsymbols('q1', 1) # Angular velocity
|
||||
L, m, t = symbols('L, m, t')
|
||||
g = 9.8
|
||||
|
||||
# Compose world frame
|
||||
N = ReferenceFrame('N')
|
||||
pN = Point('N*')
|
||||
pN.set_vel(N, 0)
|
||||
|
||||
# A.x is along the pendulum
|
||||
A = N.orientnew('A', 'axis', [q1, N.z])
|
||||
A.set_ang_vel(N, u1*N.z)
|
||||
|
||||
# Locate point P relative to the origin N*
|
||||
P = pN.locatenew('P', L*A.x)
|
||||
P.v2pt_theory(pN, N, A)
|
||||
pP = Particle('pP', P, m)
|
||||
|
||||
# Create Kinematic Differential Equations
|
||||
kde = Matrix([q1d - u1])
|
||||
|
||||
# Input the force resultant at P
|
||||
R = m*g*N.x
|
||||
|
||||
# Solve for eom with kanes method
|
||||
KM = KanesMethod(N, q_ind=[q1], u_ind=[u1], kd_eqs=kde)
|
||||
(fr, frstar) = KM.kanes_equations([pP], [(P, R)])
|
||||
|
||||
# Linearize
|
||||
A, B, inp_vec = KM.linearize(A_and_B=True, simplify=True)
|
||||
|
||||
assert A == Matrix([[0, 1], [-9.8*cos(q1)/L, 0]])
|
||||
assert B == Matrix([])
|
||||
|
||||
def test_linearize_pendulum_kane_nonminimal():
|
||||
# Create generalized coordinates and speeds for this non-minimal realization
|
||||
# q1, q2 = N.x and N.y coordinates of pendulum
|
||||
# u1, u2 = N.x and N.y velocities of pendulum
|
||||
q1, q2 = dynamicsymbols('q1:3')
|
||||
q1d, q2d = dynamicsymbols('q1:3', level=1)
|
||||
u1, u2 = dynamicsymbols('u1:3')
|
||||
u1d, u2d = dynamicsymbols('u1:3', level=1)
|
||||
L, m, t = symbols('L, m, t')
|
||||
g = 9.8
|
||||
|
||||
# Compose world frame
|
||||
N = ReferenceFrame('N')
|
||||
pN = Point('N*')
|
||||
pN.set_vel(N, 0)
|
||||
|
||||
# A.x is along the pendulum
|
||||
theta1 = atan(q2/q1)
|
||||
A = N.orientnew('A', 'axis', [theta1, N.z])
|
||||
|
||||
# Locate the pendulum mass
|
||||
P = pN.locatenew('P1', q1*N.x + q2*N.y)
|
||||
pP = Particle('pP', P, m)
|
||||
|
||||
# Calculate the kinematic differential equations
|
||||
kde = Matrix([q1d - u1,
|
||||
q2d - u2])
|
||||
dq_dict = solve(kde, [q1d, q2d])
|
||||
|
||||
# Set velocity of point P
|
||||
P.set_vel(N, P.pos_from(pN).dt(N).subs(dq_dict))
|
||||
|
||||
# Configuration constraint is length of pendulum
|
||||
f_c = Matrix([P.pos_from(pN).magnitude() - L])
|
||||
|
||||
# Velocity constraint is that the velocity in the A.x direction is
|
||||
# always zero (the pendulum is never getting longer).
|
||||
f_v = Matrix([P.vel(N).express(A).dot(A.x)])
|
||||
f_v.simplify()
|
||||
|
||||
# Acceleration constraints is the time derivative of the velocity constraint
|
||||
f_a = f_v.diff(t)
|
||||
f_a.simplify()
|
||||
|
||||
# Input the force resultant at P
|
||||
R = m*g*N.x
|
||||
|
||||
# Derive the equations of motion using the KanesMethod class.
|
||||
KM = KanesMethod(N, q_ind=[q2], u_ind=[u2], q_dependent=[q1],
|
||||
u_dependent=[u1], configuration_constraints=f_c,
|
||||
velocity_constraints=f_v, acceleration_constraints=f_a, kd_eqs=kde)
|
||||
(fr, frstar) = KM.kanes_equations([pP], [(P, R)])
|
||||
|
||||
# Set the operating point to be straight down, and non-moving
|
||||
q_op = {q1: L, q2: 0}
|
||||
u_op = {u1: 0, u2: 0}
|
||||
ud_op = {u1d: 0, u2d: 0}
|
||||
|
||||
A, B, inp_vec = KM.linearize(op_point=[q_op, u_op, ud_op], A_and_B=True,
|
||||
simplify=True)
|
||||
|
||||
assert A.expand() == Matrix([[0, 1], [-9.8/L, 0]])
|
||||
assert B == Matrix([])
|
||||
|
||||
|
||||
# symengine doesn't support method='GJ'
|
||||
A, B, inp_vec = KM.linearize(op_point=[q_op, u_op, ud_op], A_and_B=True,
|
||||
simplify=True, linear_solver='GJ')
|
||||
|
||||
assert A.expand() == Matrix([[0, 1], [-9.8/L, 0]])
|
||||
assert B == Matrix([])
|
||||
|
||||
A, B, inp_vec = KM.linearize(op_point=[q_op, u_op, ud_op],
|
||||
A_and_B=True,
|
||||
simplify=True,
|
||||
linear_solver=lambda A, b: A.LUsolve(b))
|
||||
|
||||
assert A.expand() == Matrix([[0, 1], [-9.8/L, 0]])
|
||||
assert B == Matrix([])
|
||||
|
||||
|
||||
def test_linearize_pendulum_lagrange_minimal():
|
||||
q1 = dynamicsymbols('q1') # angle of pendulum
|
||||
q1d = dynamicsymbols('q1', 1) # Angular velocity
|
||||
L, m, t = symbols('L, m, t')
|
||||
g = 9.8
|
||||
|
||||
# Compose world frame
|
||||
N = ReferenceFrame('N')
|
||||
pN = Point('N*')
|
||||
pN.set_vel(N, 0)
|
||||
|
||||
# A.x is along the pendulum
|
||||
A = N.orientnew('A', 'axis', [q1, N.z])
|
||||
A.set_ang_vel(N, q1d*N.z)
|
||||
|
||||
# Locate point P relative to the origin N*
|
||||
P = pN.locatenew('P', L*A.x)
|
||||
P.v2pt_theory(pN, N, A)
|
||||
pP = Particle('pP', P, m)
|
||||
|
||||
# Solve for eom with Lagranges method
|
||||
Lag = Lagrangian(N, pP)
|
||||
LM = LagrangesMethod(Lag, [q1], forcelist=[(P, m*g*N.x)], frame=N)
|
||||
LM.form_lagranges_equations()
|
||||
|
||||
# Linearize
|
||||
A, B, inp_vec = LM.linearize([q1], [q1d], A_and_B=True)
|
||||
|
||||
assert simplify(A) == Matrix([[0, 1], [-9.8*cos(q1)/L, 0]])
|
||||
assert B == Matrix([])
|
||||
|
||||
# Check an alternative solver
|
||||
A, B, inp_vec = LM.linearize([q1], [q1d], A_and_B=True, linear_solver='GJ')
|
||||
|
||||
assert simplify(A) == Matrix([[0, 1], [-9.8*cos(q1)/L, 0]])
|
||||
assert B == Matrix([])
|
||||
|
||||
|
||||
def test_linearize_pendulum_lagrange_nonminimal():
|
||||
q1, q2 = dynamicsymbols('q1:3')
|
||||
q1d, q2d = dynamicsymbols('q1:3', level=1)
|
||||
L, m, t = symbols('L, m, t')
|
||||
g = 9.8
|
||||
# Compose World Frame
|
||||
N = ReferenceFrame('N')
|
||||
pN = Point('N*')
|
||||
pN.set_vel(N, 0)
|
||||
# A.x is along the pendulum
|
||||
theta1 = atan(q2/q1)
|
||||
A = N.orientnew('A', 'axis', [theta1, N.z])
|
||||
# Create point P, the pendulum mass
|
||||
P = pN.locatenew('P1', q1*N.x + q2*N.y)
|
||||
P.set_vel(N, P.pos_from(pN).dt(N))
|
||||
pP = Particle('pP', P, m)
|
||||
# Constraint Equations
|
||||
f_c = Matrix([q1**2 + q2**2 - L**2])
|
||||
# Calculate the lagrangian, and form the equations of motion
|
||||
Lag = Lagrangian(N, pP)
|
||||
LM = LagrangesMethod(Lag, [q1, q2], hol_coneqs=f_c, forcelist=[(P, m*g*N.x)], frame=N)
|
||||
LM.form_lagranges_equations()
|
||||
# Compose operating point
|
||||
op_point = {q1: L, q2: 0, q1d: 0, q2d: 0, q1d.diff(t): 0, q2d.diff(t): 0}
|
||||
# Solve for multiplier operating point
|
||||
lam_op = LM.solve_multipliers(op_point=op_point)
|
||||
op_point.update(lam_op)
|
||||
# Perform the Linearization
|
||||
A, B, inp_vec = LM.linearize([q2], [q2d], [q1], [q1d],
|
||||
op_point=op_point, A_and_B=True)
|
||||
assert simplify(A) == Matrix([[0, 1], [-9.8/L, 0]])
|
||||
assert B == Matrix([])
|
||||
|
||||
# Check if passing a function to linear_solver works
|
||||
A, B, inp_vec = LM.linearize([q2], [q2d], [q1], [q1d], op_point=op_point,
|
||||
A_and_B=True, linear_solver=lambda A, b:
|
||||
A.LUsolve(b))
|
||||
assert simplify(A) == Matrix([[0, 1], [-9.8/L, 0]])
|
||||
assert B == Matrix([])
|
||||
|
||||
def test_linearize_rolling_disc_lagrange():
|
||||
q1, q2, q3 = q = dynamicsymbols('q1 q2 q3')
|
||||
q1d, q2d, q3d = qd = dynamicsymbols('q1 q2 q3', 1)
|
||||
r, m, g = symbols('r m g')
|
||||
|
||||
N = ReferenceFrame('N')
|
||||
Y = N.orientnew('Y', 'Axis', [q1, N.z])
|
||||
L = Y.orientnew('L', 'Axis', [q2, Y.x])
|
||||
R = L.orientnew('R', 'Axis', [q3, L.y])
|
||||
|
||||
C = Point('C')
|
||||
C.set_vel(N, 0)
|
||||
Dmc = C.locatenew('Dmc', r * L.z)
|
||||
Dmc.v2pt_theory(C, N, R)
|
||||
|
||||
I = inertia(L, m / 4 * r**2, m / 2 * r**2, m / 4 * r**2)
|
||||
BodyD = RigidBody('BodyD', Dmc, R, m, (I, Dmc))
|
||||
BodyD.potential_energy = - m * g * r * cos(q2)
|
||||
|
||||
Lag = Lagrangian(N, BodyD)
|
||||
l = LagrangesMethod(Lag, q)
|
||||
l.form_lagranges_equations()
|
||||
|
||||
# Linearize about steady-state upright rolling
|
||||
op_point = {q1: 0, q2: 0, q3: 0,
|
||||
q1d: 0, q2d: 0,
|
||||
q1d.diff(): 0, q2d.diff(): 0, q3d.diff(): 0}
|
||||
A = l.linearize(q_ind=q, qd_ind=qd, op_point=op_point, A_and_B=True)[0]
|
||||
sol = Matrix([[0, 0, 0, 1, 0, 0],
|
||||
[0, 0, 0, 0, 1, 0],
|
||||
[0, 0, 0, 0, 0, 1],
|
||||
[0, 0, 0, 0, -6*q3d, 0],
|
||||
[0, -4*g/(5*r), 0, 6*q3d/5, 0, 0],
|
||||
[0, 0, 0, 0, 0, 0]])
|
||||
|
||||
assert A == sol
|
||||
+86
@@ -0,0 +1,86 @@
|
||||
from pytest import raises
|
||||
|
||||
from sympy import symbols
|
||||
from sympy.physics.mechanics import (RigidBody, Particle, ReferenceFrame, Point,
|
||||
outer, dynamicsymbols, Force, Torque)
|
||||
from sympy.physics.mechanics.loads import gravity, _parse_load
|
||||
|
||||
|
||||
def test_force_default():
|
||||
N = ReferenceFrame('N')
|
||||
Po = Point('Po')
|
||||
f1 = Force(Po, N.x)
|
||||
assert f1.point == Po
|
||||
assert f1.force == N.x
|
||||
assert f1.__repr__() == 'Force(point=Po, force=N.x)'
|
||||
# Test tuple behaviour
|
||||
assert isinstance(f1, tuple)
|
||||
assert f1[0] == Po
|
||||
assert f1[1] == N.x
|
||||
assert f1 == (Po, N.x)
|
||||
assert f1 != (N.x, Po)
|
||||
assert f1 != (Po, N.x + N.y)
|
||||
assert f1 != (Point('Co'), N.x)
|
||||
# Test body as input
|
||||
P = Particle('P', Po)
|
||||
f2 = Force(P, N.x)
|
||||
assert f1 == f2
|
||||
|
||||
|
||||
def test_torque_default():
|
||||
N = ReferenceFrame('N')
|
||||
f1 = Torque(N, N.x)
|
||||
assert f1.frame == N
|
||||
assert f1.torque == N.x
|
||||
assert f1.__repr__() == 'Torque(frame=N, torque=N.x)'
|
||||
# Test tuple behaviour
|
||||
assert isinstance(f1, tuple)
|
||||
assert f1[0] == N
|
||||
assert f1[1] == N.x
|
||||
assert f1 == (N, N.x)
|
||||
assert f1 != (N.x, N)
|
||||
assert f1 != (N, N.x + N.y)
|
||||
assert f1 != (ReferenceFrame('A'), N.x)
|
||||
# Test body as input
|
||||
rb = RigidBody('P', frame=N)
|
||||
f2 = Torque(rb, N.x)
|
||||
assert f1 == f2
|
||||
|
||||
|
||||
def test_gravity():
|
||||
N = ReferenceFrame('N')
|
||||
m, M, g = symbols('m M g')
|
||||
F1, F2 = dynamicsymbols('F1 F2')
|
||||
po = Point('po')
|
||||
pa = Particle('pa', po, m)
|
||||
A = ReferenceFrame('A')
|
||||
P = Point('P')
|
||||
I = outer(A.x, A.x)
|
||||
B = RigidBody('B', P, A, M, (I, P))
|
||||
forceList = [(po, F1), (P, F2)]
|
||||
forceList.extend(gravity(g * N.y, pa, B))
|
||||
l = [(po, F1), (P, F2), (po, g * m * N.y), (P, g * M * N.y)]
|
||||
|
||||
for i in range(len(l)):
|
||||
for j in range(len(l[i])):
|
||||
assert forceList[i][j] == l[i][j]
|
||||
|
||||
|
||||
def test_parse_loads():
|
||||
N = ReferenceFrame('N')
|
||||
po = Point('po')
|
||||
assert _parse_load(Force(po, N.z)) == (po, N.z)
|
||||
assert _parse_load(Torque(N, N.x)) == (N, N.x)
|
||||
f1 = _parse_load((po, N.x)) # Test whether a force is recognized
|
||||
assert isinstance(f1, Force)
|
||||
assert f1 == Force(po, N.x)
|
||||
t1 = _parse_load((N, N.y)) # Test whether a torque is recognized
|
||||
assert isinstance(t1, Torque)
|
||||
assert t1 == Torque(N, N.y)
|
||||
# Bodies should be undetermined (even in case of a Particle)
|
||||
raises(ValueError, lambda: _parse_load((Particle('pa', po), N.x)))
|
||||
raises(ValueError, lambda: _parse_load((RigidBody('pa', po, N), N.x)))
|
||||
# Invalid tuple length
|
||||
raises(ValueError, lambda: _parse_load((po, N.x, po, N.x)))
|
||||
# Invalid type
|
||||
raises(TypeError, lambda: _parse_load([po, N.x]))
|
||||
+5
@@ -0,0 +1,5 @@
|
||||
from sympy.physics.mechanics.method import _Methods
|
||||
from sympy.testing.pytest import raises
|
||||
|
||||
def test_method():
|
||||
raises(TypeError, lambda: _Methods())
|
||||
+117
@@ -0,0 +1,117 @@
|
||||
import sympy.physics.mechanics.models as models
|
||||
from sympy import (cos, sin, Matrix, symbols, zeros)
|
||||
from sympy.simplify.simplify import simplify
|
||||
from sympy.physics.mechanics import (dynamicsymbols)
|
||||
|
||||
|
||||
def test_multi_mass_spring_damper_inputs():
|
||||
|
||||
c0, k0, m0 = symbols("c0 k0 m0")
|
||||
g = symbols("g")
|
||||
v0, x0, f0 = dynamicsymbols("v0 x0 f0")
|
||||
|
||||
kane1 = models.multi_mass_spring_damper(1)
|
||||
massmatrix1 = Matrix([[m0]])
|
||||
forcing1 = Matrix([[-c0*v0 - k0*x0]])
|
||||
assert simplify(massmatrix1 - kane1.mass_matrix) == Matrix([0])
|
||||
assert simplify(forcing1 - kane1.forcing) == Matrix([0])
|
||||
|
||||
kane2 = models.multi_mass_spring_damper(1, True)
|
||||
massmatrix2 = Matrix([[m0]])
|
||||
forcing2 = Matrix([[-c0*v0 + g*m0 - k0*x0]])
|
||||
assert simplify(massmatrix2 - kane2.mass_matrix) == Matrix([0])
|
||||
assert simplify(forcing2 - kane2.forcing) == Matrix([0])
|
||||
|
||||
kane3 = models.multi_mass_spring_damper(1, True, True)
|
||||
massmatrix3 = Matrix([[m0]])
|
||||
forcing3 = Matrix([[-c0*v0 + g*m0 - k0*x0 + f0]])
|
||||
assert simplify(massmatrix3 - kane3.mass_matrix) == Matrix([0])
|
||||
assert simplify(forcing3 - kane3.forcing) == Matrix([0])
|
||||
|
||||
kane4 = models.multi_mass_spring_damper(1, False, True)
|
||||
massmatrix4 = Matrix([[m0]])
|
||||
forcing4 = Matrix([[-c0*v0 - k0*x0 + f0]])
|
||||
assert simplify(massmatrix4 - kane4.mass_matrix) == Matrix([0])
|
||||
assert simplify(forcing4 - kane4.forcing) == Matrix([0])
|
||||
|
||||
|
||||
def test_multi_mass_spring_damper_higher_order():
|
||||
c0, k0, m0 = symbols("c0 k0 m0")
|
||||
c1, k1, m1 = symbols("c1 k1 m1")
|
||||
c2, k2, m2 = symbols("c2 k2 m2")
|
||||
v0, x0 = dynamicsymbols("v0 x0")
|
||||
v1, x1 = dynamicsymbols("v1 x1")
|
||||
v2, x2 = dynamicsymbols("v2 x2")
|
||||
|
||||
kane1 = models.multi_mass_spring_damper(3)
|
||||
massmatrix1 = Matrix([[m0 + m1 + m2, m1 + m2, m2],
|
||||
[m1 + m2, m1 + m2, m2],
|
||||
[m2, m2, m2]])
|
||||
forcing1 = Matrix([[-c0*v0 - k0*x0],
|
||||
[-c1*v1 - k1*x1],
|
||||
[-c2*v2 - k2*x2]])
|
||||
assert simplify(massmatrix1 - kane1.mass_matrix) == zeros(3)
|
||||
assert simplify(forcing1 - kane1.forcing) == Matrix([0, 0, 0])
|
||||
|
||||
|
||||
def test_n_link_pendulum_on_cart_inputs():
|
||||
l0, m0 = symbols("l0 m0")
|
||||
m1 = symbols("m1")
|
||||
g = symbols("g")
|
||||
q0, q1, F, T1 = dynamicsymbols("q0 q1 F T1")
|
||||
u0, u1 = dynamicsymbols("u0 u1")
|
||||
|
||||
kane1 = models.n_link_pendulum_on_cart(1)
|
||||
massmatrix1 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
|
||||
[-l0*m1*cos(q1), l0**2*m1]])
|
||||
forcing1 = Matrix([[-l0*m1*u1**2*sin(q1) + F], [g*l0*m1*sin(q1)]])
|
||||
assert simplify(massmatrix1 - kane1.mass_matrix) == zeros(2)
|
||||
assert simplify(forcing1 - kane1.forcing) == Matrix([0, 0])
|
||||
|
||||
kane2 = models.n_link_pendulum_on_cart(1, False)
|
||||
massmatrix2 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
|
||||
[-l0*m1*cos(q1), l0**2*m1]])
|
||||
forcing2 = Matrix([[-l0*m1*u1**2*sin(q1)], [g*l0*m1*sin(q1)]])
|
||||
assert simplify(massmatrix2 - kane2.mass_matrix) == zeros(2)
|
||||
assert simplify(forcing2 - kane2.forcing) == Matrix([0, 0])
|
||||
|
||||
kane3 = models.n_link_pendulum_on_cart(1, False, True)
|
||||
massmatrix3 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
|
||||
[-l0*m1*cos(q1), l0**2*m1]])
|
||||
forcing3 = Matrix([[-l0*m1*u1**2*sin(q1)], [g*l0*m1*sin(q1) + T1]])
|
||||
assert simplify(massmatrix3 - kane3.mass_matrix) == zeros(2)
|
||||
assert simplify(forcing3 - kane3.forcing) == Matrix([0, 0])
|
||||
|
||||
kane4 = models.n_link_pendulum_on_cart(1, True, False)
|
||||
massmatrix4 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
|
||||
[-l0*m1*cos(q1), l0**2*m1]])
|
||||
forcing4 = Matrix([[-l0*m1*u1**2*sin(q1) + F], [g*l0*m1*sin(q1)]])
|
||||
assert simplify(massmatrix4 - kane4.mass_matrix) == zeros(2)
|
||||
assert simplify(forcing4 - kane4.forcing) == Matrix([0, 0])
|
||||
|
||||
|
||||
def test_n_link_pendulum_on_cart_higher_order():
|
||||
l0, m0 = symbols("l0 m0")
|
||||
l1, m1 = symbols("l1 m1")
|
||||
m2 = symbols("m2")
|
||||
g = symbols("g")
|
||||
q0, q1, q2 = dynamicsymbols("q0 q1 q2")
|
||||
u0, u1, u2 = dynamicsymbols("u0 u1 u2")
|
||||
F, T1 = dynamicsymbols("F T1")
|
||||
|
||||
kane1 = models.n_link_pendulum_on_cart(2)
|
||||
massmatrix1 = Matrix([[m0 + m1 + m2, -l0*m1*cos(q1) - l0*m2*cos(q1),
|
||||
-l1*m2*cos(q2)],
|
||||
[-l0*m1*cos(q1) - l0*m2*cos(q1), l0**2*m1 + l0**2*m2,
|
||||
l0*l1*m2*(sin(q1)*sin(q2) + cos(q1)*cos(q2))],
|
||||
[-l1*m2*cos(q2),
|
||||
l0*l1*m2*(sin(q1)*sin(q2) + cos(q1)*cos(q2)),
|
||||
l1**2*m2]])
|
||||
forcing1 = Matrix([[-l0*m1*u1**2*sin(q1) - l0*m2*u1**2*sin(q1) -
|
||||
l1*m2*u2**2*sin(q2) + F],
|
||||
[g*l0*m1*sin(q1) + g*l0*m2*sin(q1) -
|
||||
l0*l1*m2*(sin(q1)*cos(q2) - sin(q2)*cos(q1))*u2**2],
|
||||
[g*l1*m2*sin(q2) - l0*l1*m2*(-sin(q1)*cos(q2) +
|
||||
sin(q2)*cos(q1))*u1**2]])
|
||||
assert simplify(massmatrix1 - kane1.mass_matrix) == zeros(3)
|
||||
assert simplify(forcing1 - kane1.forcing) == Matrix([0, 0, 0])
|
||||
+78
@@ -0,0 +1,78 @@
|
||||
from sympy import symbols
|
||||
from sympy.physics.mechanics import Point, Particle, ReferenceFrame, inertia
|
||||
from sympy.physics.mechanics.body_base import BodyBase
|
||||
from sympy.testing.pytest import raises, warns_deprecated_sympy
|
||||
|
||||
|
||||
def test_particle_default():
|
||||
# Test default
|
||||
p = Particle('P')
|
||||
assert p.name == 'P'
|
||||
assert p.mass == symbols('P_mass')
|
||||
assert p.masscenter.name == 'P_masscenter'
|
||||
assert p.potential_energy == 0
|
||||
assert p.__str__() == 'P'
|
||||
assert p.__repr__() == ("Particle('P', masscenter=P_masscenter, "
|
||||
"mass=P_mass)")
|
||||
raises(AttributeError, lambda: p.frame)
|
||||
|
||||
|
||||
def test_particle():
|
||||
# Test initializing with parameters
|
||||
m, m2, v1, v2, v3, r, g, h = symbols('m m2 v1 v2 v3 r g h')
|
||||
P = Point('P')
|
||||
P2 = Point('P2')
|
||||
p = Particle('pa', P, m)
|
||||
assert isinstance(p, BodyBase)
|
||||
assert p.mass == m
|
||||
assert p.point == P
|
||||
# Test the mass setter
|
||||
p.mass = m2
|
||||
assert p.mass == m2
|
||||
# Test the point setter
|
||||
p.point = P2
|
||||
assert p.point == P2
|
||||
# Test the linear momentum function
|
||||
N = ReferenceFrame('N')
|
||||
O = Point('O')
|
||||
P2.set_pos(O, r * N.y)
|
||||
P2.set_vel(N, v1 * N.x)
|
||||
raises(TypeError, lambda: Particle(P, P, m))
|
||||
raises(TypeError, lambda: Particle('pa', m, m))
|
||||
assert p.linear_momentum(N) == m2 * v1 * N.x
|
||||
assert p.angular_momentum(O, N) == -m2 * r * v1 * N.z
|
||||
P2.set_vel(N, v2 * N.y)
|
||||
assert p.linear_momentum(N) == m2 * v2 * N.y
|
||||
assert p.angular_momentum(O, N) == 0
|
||||
P2.set_vel(N, v3 * N.z)
|
||||
assert p.linear_momentum(N) == m2 * v3 * N.z
|
||||
assert p.angular_momentum(O, N) == m2 * r * v3 * N.x
|
||||
P2.set_vel(N, v1 * N.x + v2 * N.y + v3 * N.z)
|
||||
assert p.linear_momentum(N) == m2 * (v1 * N.x + v2 * N.y + v3 * N.z)
|
||||
assert p.angular_momentum(O, N) == m2 * r * (v3 * N.x - v1 * N.z)
|
||||
p.potential_energy = m * g * h
|
||||
assert p.potential_energy == m * g * h
|
||||
# TODO make the result not be system-dependent
|
||||
assert p.kinetic_energy(
|
||||
N) in [m2 * (v1 ** 2 + v2 ** 2 + v3 ** 2) / 2,
|
||||
m2 * v1 ** 2 / 2 + m2 * v2 ** 2 / 2 + m2 * v3 ** 2 / 2]
|
||||
|
||||
|
||||
def test_parallel_axis():
|
||||
N = ReferenceFrame('N')
|
||||
m, a, b = symbols('m, a, b')
|
||||
o = Point('o')
|
||||
p = o.locatenew('p', a * N.x + b * N.y)
|
||||
P = Particle('P', o, m)
|
||||
Ip = P.parallel_axis(p, N)
|
||||
Ip_expected = inertia(N, m * b ** 2, m * a ** 2, m * (a ** 2 + b ** 2),
|
||||
ixy=-m * a * b)
|
||||
assert Ip == Ip_expected
|
||||
|
||||
|
||||
def test_deprecated_set_potential_energy():
|
||||
m, g, h = symbols('m g h')
|
||||
P = Point('P')
|
||||
p = Particle('pa', P, m)
|
||||
with warns_deprecated_sympy():
|
||||
p.set_potential_energy(m * g * h)
|
||||
+691
@@ -0,0 +1,691 @@
|
||||
"""Tests for the ``sympy.physics.mechanics.pathway.py`` module."""
|
||||
|
||||
import pytest
|
||||
|
||||
from sympy import (
|
||||
Rational,
|
||||
Symbol,
|
||||
cos,
|
||||
pi,
|
||||
sin,
|
||||
sqrt,
|
||||
)
|
||||
from sympy.physics.mechanics import (
|
||||
Force,
|
||||
LinearPathway,
|
||||
ObstacleSetPathway,
|
||||
PathwayBase,
|
||||
Point,
|
||||
ReferenceFrame,
|
||||
WrappingCylinder,
|
||||
WrappingGeometryBase,
|
||||
WrappingPathway,
|
||||
WrappingSphere,
|
||||
dynamicsymbols,
|
||||
)
|
||||
from sympy.simplify.simplify import simplify
|
||||
|
||||
|
||||
def _simplify_loads(loads):
|
||||
return [
|
||||
load.__class__(load.location, load.vector.simplify())
|
||||
for load in loads
|
||||
]
|
||||
|
||||
|
||||
class TestLinearPathway:
|
||||
|
||||
def test_is_pathway_base_subclass(self):
|
||||
assert issubclass(LinearPathway, PathwayBase)
|
||||
|
||||
@staticmethod
|
||||
@pytest.mark.parametrize(
|
||||
'args, kwargs',
|
||||
[
|
||||
((Point('pA'), Point('pB')), {}),
|
||||
]
|
||||
)
|
||||
def test_valid_constructor(args, kwargs):
|
||||
pointA, pointB = args
|
||||
instance = LinearPathway(*args, **kwargs)
|
||||
assert isinstance(instance, LinearPathway)
|
||||
assert hasattr(instance, 'attachments')
|
||||
assert len(instance.attachments) == 2
|
||||
assert instance.attachments[0] is pointA
|
||||
assert instance.attachments[1] is pointB
|
||||
assert isinstance(instance.attachments[0], Point)
|
||||
assert instance.attachments[0].name == 'pA'
|
||||
assert isinstance(instance.attachments[1], Point)
|
||||
assert instance.attachments[1].name == 'pB'
|
||||
|
||||
@staticmethod
|
||||
@pytest.mark.parametrize(
|
||||
'attachments',
|
||||
[
|
||||
(Point('pA'), ),
|
||||
(Point('pA'), Point('pB'), Point('pZ')),
|
||||
]
|
||||
)
|
||||
def test_invalid_attachments_incorrect_number(attachments):
|
||||
with pytest.raises(ValueError):
|
||||
_ = LinearPathway(*attachments)
|
||||
|
||||
@staticmethod
|
||||
@pytest.mark.parametrize(
|
||||
'attachments',
|
||||
[
|
||||
(None, Point('pB')),
|
||||
(Point('pA'), None),
|
||||
]
|
||||
)
|
||||
def test_invalid_attachments_not_point(attachments):
|
||||
with pytest.raises(TypeError):
|
||||
_ = LinearPathway(*attachments)
|
||||
|
||||
@pytest.fixture(autouse=True)
|
||||
def _linear_pathway_fixture(self):
|
||||
self.N = ReferenceFrame('N')
|
||||
self.pA = Point('pA')
|
||||
self.pB = Point('pB')
|
||||
self.pathway = LinearPathway(self.pA, self.pB)
|
||||
self.q1 = dynamicsymbols('q1')
|
||||
self.q2 = dynamicsymbols('q2')
|
||||
self.q3 = dynamicsymbols('q3')
|
||||
self.q1d = dynamicsymbols('q1', 1)
|
||||
self.q2d = dynamicsymbols('q2', 1)
|
||||
self.q3d = dynamicsymbols('q3', 1)
|
||||
self.F = Symbol('F')
|
||||
|
||||
def test_properties_are_immutable(self):
|
||||
instance = LinearPathway(self.pA, self.pB)
|
||||
with pytest.raises(AttributeError):
|
||||
instance.attachments = None
|
||||
with pytest.raises(TypeError):
|
||||
instance.attachments[0] = None
|
||||
with pytest.raises(TypeError):
|
||||
instance.attachments[1] = None
|
||||
|
||||
def test_repr(self):
|
||||
pathway = LinearPathway(self.pA, self.pB)
|
||||
expected = 'LinearPathway(pA, pB)'
|
||||
assert repr(pathway) == expected
|
||||
|
||||
def test_static_pathway_length(self):
|
||||
self.pB.set_pos(self.pA, 2*self.N.x)
|
||||
assert self.pathway.length == 2
|
||||
|
||||
def test_static_pathway_extension_velocity(self):
|
||||
self.pB.set_pos(self.pA, 2*self.N.x)
|
||||
assert self.pathway.extension_velocity == 0
|
||||
|
||||
def test_static_pathway_to_loads(self):
|
||||
self.pB.set_pos(self.pA, 2*self.N.x)
|
||||
expected = [
|
||||
(self.pA, - self.F*self.N.x),
|
||||
(self.pB, self.F*self.N.x),
|
||||
]
|
||||
assert self.pathway.to_loads(self.F) == expected
|
||||
|
||||
def test_2D_pathway_length(self):
|
||||
self.pB.set_pos(self.pA, 2*self.q1*self.N.x)
|
||||
expected = 2*sqrt(self.q1**2)
|
||||
assert self.pathway.length == expected
|
||||
|
||||
def test_2D_pathway_extension_velocity(self):
|
||||
self.pB.set_pos(self.pA, 2*self.q1*self.N.x)
|
||||
expected = 2*sqrt(self.q1**2)*self.q1d/self.q1
|
||||
assert self.pathway.extension_velocity == expected
|
||||
|
||||
def test_2D_pathway_to_loads(self):
|
||||
self.pB.set_pos(self.pA, 2*self.q1*self.N.x)
|
||||
expected = [
|
||||
(self.pA, - self.F*(self.q1 / sqrt(self.q1**2))*self.N.x),
|
||||
(self.pB, self.F*(self.q1 / sqrt(self.q1**2))*self.N.x),
|
||||
]
|
||||
assert self.pathway.to_loads(self.F) == expected
|
||||
|
||||
def test_3D_pathway_length(self):
|
||||
self.pB.set_pos(
|
||||
self.pA,
|
||||
self.q1*self.N.x - self.q2*self.N.y + 2*self.q3*self.N.z,
|
||||
)
|
||||
expected = sqrt(self.q1**2 + self.q2**2 + 4*self.q3**2)
|
||||
assert simplify(self.pathway.length - expected) == 0
|
||||
|
||||
def test_3D_pathway_extension_velocity(self):
|
||||
self.pB.set_pos(
|
||||
self.pA,
|
||||
self.q1*self.N.x - self.q2*self.N.y + 2*self.q3*self.N.z,
|
||||
)
|
||||
length = sqrt(self.q1**2 + self.q2**2 + 4*self.q3**2)
|
||||
expected = (
|
||||
self.q1*self.q1d/length
|
||||
+ self.q2*self.q2d/length
|
||||
+ 4*self.q3*self.q3d/length
|
||||
)
|
||||
assert simplify(self.pathway.extension_velocity - expected) == 0
|
||||
|
||||
def test_3D_pathway_to_loads(self):
|
||||
self.pB.set_pos(
|
||||
self.pA,
|
||||
self.q1*self.N.x - self.q2*self.N.y + 2*self.q3*self.N.z,
|
||||
)
|
||||
length = sqrt(self.q1**2 + self.q2**2 + 4*self.q3**2)
|
||||
pO_force = (
|
||||
- self.F*self.q1*self.N.x/length
|
||||
+ self.F*self.q2*self.N.y/length
|
||||
- 2*self.F*self.q3*self.N.z/length
|
||||
)
|
||||
pI_force = (
|
||||
self.F*self.q1*self.N.x/length
|
||||
- self.F*self.q2*self.N.y/length
|
||||
+ 2*self.F*self.q3*self.N.z/length
|
||||
)
|
||||
expected = [
|
||||
(self.pA, pO_force),
|
||||
(self.pB, pI_force),
|
||||
]
|
||||
assert self.pathway.to_loads(self.F) == expected
|
||||
|
||||
|
||||
class TestObstacleSetPathway:
|
||||
|
||||
def test_is_pathway_base_subclass(self):
|
||||
assert issubclass(ObstacleSetPathway, PathwayBase)
|
||||
|
||||
@staticmethod
|
||||
@pytest.mark.parametrize(
|
||||
'num_attachments, attachments',
|
||||
[
|
||||
(3, [Point(name) for name in ('pO', 'pA', 'pI')]),
|
||||
(4, [Point(name) for name in ('pO', 'pA', 'pB', 'pI')]),
|
||||
(5, [Point(name) for name in ('pO', 'pA', 'pB', 'pC', 'pI')]),
|
||||
(6, [Point(name) for name in ('pO', 'pA', 'pB', 'pC', 'pD', 'pI')]),
|
||||
]
|
||||
)
|
||||
def test_valid_constructor(num_attachments, attachments):
|
||||
instance = ObstacleSetPathway(*attachments)
|
||||
assert isinstance(instance, ObstacleSetPathway)
|
||||
assert hasattr(instance, 'attachments')
|
||||
assert len(instance.attachments) == num_attachments
|
||||
for attachment in instance.attachments:
|
||||
assert isinstance(attachment, Point)
|
||||
|
||||
@staticmethod
|
||||
@pytest.mark.parametrize(
|
||||
'attachments',
|
||||
[[Point('pO')], [Point('pO'), Point('pI')]],
|
||||
)
|
||||
def test_invalid_constructor_attachments_incorrect_number(attachments):
|
||||
with pytest.raises(ValueError):
|
||||
_ = ObstacleSetPathway(*attachments)
|
||||
|
||||
@staticmethod
|
||||
@pytest.mark.parametrize(
|
||||
'attachments',
|
||||
[
|
||||
(None, Point('pA'), Point('pI')),
|
||||
(Point('pO'), None, Point('pI')),
|
||||
(Point('pO'), Point('pA'), None),
|
||||
]
|
||||
)
|
||||
def test_invalid_constructor_attachments_not_point(attachments):
|
||||
with pytest.raises(TypeError):
|
||||
_ = WrappingPathway(*attachments) # type: ignore
|
||||
|
||||
def test_properties_are_immutable(self):
|
||||
pathway = ObstacleSetPathway(Point('pO'), Point('pA'), Point('pI'))
|
||||
with pytest.raises(AttributeError):
|
||||
pathway.attachments = None # type: ignore
|
||||
with pytest.raises(TypeError):
|
||||
pathway.attachments[0] = None # type: ignore
|
||||
with pytest.raises(TypeError):
|
||||
pathway.attachments[1] = None # type: ignore
|
||||
with pytest.raises(TypeError):
|
||||
pathway.attachments[-1] = None # type: ignore
|
||||
|
||||
@staticmethod
|
||||
@pytest.mark.parametrize(
|
||||
'attachments, expected',
|
||||
[
|
||||
(
|
||||
[Point(name) for name in ('pO', 'pA', 'pI')],
|
||||
'ObstacleSetPathway(pO, pA, pI)'
|
||||
),
|
||||
(
|
||||
[Point(name) for name in ('pO', 'pA', 'pB', 'pI')],
|
||||
'ObstacleSetPathway(pO, pA, pB, pI)'
|
||||
),
|
||||
(
|
||||
[Point(name) for name in ('pO', 'pA', 'pB', 'pC', 'pI')],
|
||||
'ObstacleSetPathway(pO, pA, pB, pC, pI)'
|
||||
),
|
||||
]
|
||||
)
|
||||
def test_repr(attachments, expected):
|
||||
pathway = ObstacleSetPathway(*attachments)
|
||||
assert repr(pathway) == expected
|
||||
|
||||
@pytest.fixture(autouse=True)
|
||||
def _obstacle_set_pathway_fixture(self):
|
||||
self.N = ReferenceFrame('N')
|
||||
self.pO = Point('pO')
|
||||
self.pI = Point('pI')
|
||||
self.pA = Point('pA')
|
||||
self.pB = Point('pB')
|
||||
self.q = dynamicsymbols('q')
|
||||
self.qd = dynamicsymbols('q', 1)
|
||||
self.F = Symbol('F')
|
||||
|
||||
def test_static_pathway_length(self):
|
||||
self.pA.set_pos(self.pO, self.N.x)
|
||||
self.pB.set_pos(self.pO, self.N.y)
|
||||
self.pI.set_pos(self.pO, self.N.z)
|
||||
pathway = ObstacleSetPathway(self.pO, self.pA, self.pB, self.pI)
|
||||
assert pathway.length == 1 + 2 * sqrt(2)
|
||||
|
||||
def test_static_pathway_extension_velocity(self):
|
||||
self.pA.set_pos(self.pO, self.N.x)
|
||||
self.pB.set_pos(self.pO, self.N.y)
|
||||
self.pI.set_pos(self.pO, self.N.z)
|
||||
pathway = ObstacleSetPathway(self.pO, self.pA, self.pB, self.pI)
|
||||
assert pathway.extension_velocity == 0
|
||||
|
||||
def test_static_pathway_to_loads(self):
|
||||
self.pA.set_pos(self.pO, self.N.x)
|
||||
self.pB.set_pos(self.pO, self.N.y)
|
||||
self.pI.set_pos(self.pO, self.N.z)
|
||||
pathway = ObstacleSetPathway(self.pO, self.pA, self.pB, self.pI)
|
||||
expected = [
|
||||
Force(self.pO, -self.F * self.N.x),
|
||||
Force(self.pA, self.F * self.N.x),
|
||||
Force(self.pA, self.F * sqrt(2) / 2 * (self.N.x - self.N.y)),
|
||||
Force(self.pB, self.F * sqrt(2) / 2 * (self.N.y - self.N.x)),
|
||||
Force(self.pB, self.F * sqrt(2) / 2 * (self.N.y - self.N.z)),
|
||||
Force(self.pI, self.F * sqrt(2) / 2 * (self.N.z - self.N.y)),
|
||||
]
|
||||
assert pathway.to_loads(self.F) == expected
|
||||
|
||||
def test_2D_pathway_length(self):
|
||||
self.pA.set_pos(self.pO, -(self.N.x + self.N.y))
|
||||
self.pB.set_pos(
|
||||
self.pO, cos(self.q) * self.N.x - (sin(self.q) + 1) * self.N.y
|
||||
)
|
||||
self.pI.set_pos(
|
||||
self.pO, sin(self.q) * self.N.x + (cos(self.q) - 1) * self.N.y
|
||||
)
|
||||
pathway = ObstacleSetPathway(self.pO, self.pA, self.pB, self.pI)
|
||||
expected = 2 * sqrt(2) + sqrt(2 + 2*cos(self.q))
|
||||
assert (pathway.length - expected).simplify() == 0
|
||||
|
||||
def test_2D_pathway_extension_velocity(self):
|
||||
self.pA.set_pos(self.pO, -(self.N.x + self.N.y))
|
||||
self.pB.set_pos(
|
||||
self.pO, cos(self.q) * self.N.x - (sin(self.q) + 1) * self.N.y
|
||||
)
|
||||
self.pI.set_pos(
|
||||
self.pO, sin(self.q) * self.N.x + (cos(self.q) - 1) * self.N.y
|
||||
)
|
||||
pathway = ObstacleSetPathway(self.pO, self.pA, self.pB, self.pI)
|
||||
expected = - (sqrt(2) * sin(self.q) * self.qd) / (2 * sqrt(cos(self.q) + 1))
|
||||
assert (pathway.extension_velocity - expected).simplify() == 0
|
||||
|
||||
def test_2D_pathway_to_loads(self):
|
||||
self.pA.set_pos(self.pO, -(self.N.x + self.N.y))
|
||||
self.pB.set_pos(
|
||||
self.pO, cos(self.q) * self.N.x - (sin(self.q) + 1) * self.N.y
|
||||
)
|
||||
self.pI.set_pos(
|
||||
self.pO, sin(self.q) * self.N.x + (cos(self.q) - 1) * self.N.y
|
||||
)
|
||||
pathway = ObstacleSetPathway(self.pO, self.pA, self.pB, self.pI)
|
||||
pO_pA_force_vec = sqrt(2) / 2 * (self.N.x + self.N.y)
|
||||
pA_pB_force_vec = (
|
||||
- sqrt(2 * cos(self.q) + 2) / 2 * self.N.x
|
||||
+ sqrt(2) * sin(self.q) / (2 * sqrt(cos(self.q) + 1)) * self.N.y
|
||||
)
|
||||
pB_pI_force_vec = cos(self.q + pi/4) * self.N.x - sin(self.q + pi/4) * self.N.y
|
||||
expected = [
|
||||
Force(self.pO, self.F * pO_pA_force_vec),
|
||||
Force(self.pA, -self.F * pO_pA_force_vec),
|
||||
Force(self.pA, self.F * pA_pB_force_vec),
|
||||
Force(self.pB, -self.F * pA_pB_force_vec),
|
||||
Force(self.pB, self.F * pB_pI_force_vec),
|
||||
Force(self.pI, -self.F * pB_pI_force_vec),
|
||||
]
|
||||
assert _simplify_loads(pathway.to_loads(self.F)) == expected
|
||||
|
||||
|
||||
class TestWrappingPathway:
|
||||
|
||||
def test_is_pathway_base_subclass(self):
|
||||
assert issubclass(WrappingPathway, PathwayBase)
|
||||
|
||||
@pytest.fixture(autouse=True)
|
||||
def _wrapping_pathway_fixture(self):
|
||||
self.pA = Point('pA')
|
||||
self.pB = Point('pB')
|
||||
self.r = Symbol('r', positive=True)
|
||||
self.pO = Point('pO')
|
||||
self.N = ReferenceFrame('N')
|
||||
self.ax = self.N.z
|
||||
self.sphere = WrappingSphere(self.r, self.pO)
|
||||
self.cylinder = WrappingCylinder(self.r, self.pO, self.ax)
|
||||
self.pathway = WrappingPathway(self.pA, self.pB, self.cylinder)
|
||||
self.F = Symbol('F')
|
||||
|
||||
def test_valid_constructor(self):
|
||||
instance = WrappingPathway(self.pA, self.pB, self.cylinder)
|
||||
assert isinstance(instance, WrappingPathway)
|
||||
assert hasattr(instance, 'attachments')
|
||||
assert len(instance.attachments) == 2
|
||||
assert isinstance(instance.attachments[0], Point)
|
||||
assert instance.attachments[0] == self.pA
|
||||
assert isinstance(instance.attachments[1], Point)
|
||||
assert instance.attachments[1] == self.pB
|
||||
assert hasattr(instance, 'geometry')
|
||||
assert isinstance(instance.geometry, WrappingGeometryBase)
|
||||
assert instance.geometry == self.cylinder
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
'attachments',
|
||||
[
|
||||
(Point('pA'), ),
|
||||
(Point('pA'), Point('pB'), Point('pZ')),
|
||||
]
|
||||
)
|
||||
def test_invalid_constructor_attachments_incorrect_number(self, attachments):
|
||||
with pytest.raises(TypeError):
|
||||
_ = WrappingPathway(*attachments, self.cylinder)
|
||||
|
||||
@staticmethod
|
||||
@pytest.mark.parametrize(
|
||||
'attachments',
|
||||
[
|
||||
(None, Point('pB')),
|
||||
(Point('pA'), None),
|
||||
]
|
||||
)
|
||||
def test_invalid_constructor_attachments_not_point(attachments):
|
||||
with pytest.raises(TypeError):
|
||||
_ = WrappingPathway(*attachments)
|
||||
|
||||
def test_invalid_constructor_geometry_is_not_supplied(self):
|
||||
with pytest.raises(TypeError):
|
||||
_ = WrappingPathway(self.pA, self.pB)
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
'geometry',
|
||||
[
|
||||
Symbol('r'),
|
||||
dynamicsymbols('q'),
|
||||
ReferenceFrame('N'),
|
||||
ReferenceFrame('N').x,
|
||||
]
|
||||
)
|
||||
def test_invalid_geometry_not_geometry(self, geometry):
|
||||
with pytest.raises(TypeError):
|
||||
_ = WrappingPathway(self.pA, self.pB, geometry)
|
||||
|
||||
def test_attachments_property_is_immutable(self):
|
||||
with pytest.raises(TypeError):
|
||||
self.pathway.attachments[0] = self.pB
|
||||
with pytest.raises(TypeError):
|
||||
self.pathway.attachments[1] = self.pA
|
||||
|
||||
def test_geometry_property_is_immutable(self):
|
||||
with pytest.raises(AttributeError):
|
||||
self.pathway.geometry = None
|
||||
|
||||
def test_repr(self):
|
||||
expected = (
|
||||
f'WrappingPathway(pA, pB, '
|
||||
f'geometry={self.cylinder!r})'
|
||||
)
|
||||
assert repr(self.pathway) == expected
|
||||
|
||||
@staticmethod
|
||||
def _expand_pos_to_vec(pos, frame):
|
||||
return sum(mag*unit for (mag, unit) in zip(pos, frame))
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
'pA_vec, pB_vec, factor',
|
||||
[
|
||||
((1, 0, 0), (0, 1, 0), pi/2),
|
||||
((0, 1, 0), (sqrt(2)/2, -sqrt(2)/2, 0), 3*pi/4),
|
||||
((1, 0, 0), (Rational(1, 2), sqrt(3)/2, 0), pi/3),
|
||||
]
|
||||
)
|
||||
def test_static_pathway_on_sphere_length(self, pA_vec, pB_vec, factor):
|
||||
pA_vec = self._expand_pos_to_vec(pA_vec, self.N)
|
||||
pB_vec = self._expand_pos_to_vec(pB_vec, self.N)
|
||||
self.pA.set_pos(self.pO, self.r*pA_vec)
|
||||
self.pB.set_pos(self.pO, self.r*pB_vec)
|
||||
pathway = WrappingPathway(self.pA, self.pB, self.sphere)
|
||||
expected = factor*self.r
|
||||
assert simplify(pathway.length - expected) == 0
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
'pA_vec, pB_vec, factor',
|
||||
[
|
||||
((1, 0, 0), (0, 1, 0), Rational(1, 2)*pi),
|
||||
((1, 0, 0), (-1, 0, 0), pi),
|
||||
((-1, 0, 0), (1, 0, 0), pi),
|
||||
((0, 1, 0), (sqrt(2)/2, -sqrt(2)/2, 0), 5*pi/4),
|
||||
((1, 0, 0), (Rational(1, 2), sqrt(3)/2, 0), pi/3),
|
||||
(
|
||||
(0, 1, 0),
|
||||
(sqrt(2)*Rational(1, 2), -sqrt(2)*Rational(1, 2), 1),
|
||||
sqrt(1 + (Rational(5, 4)*pi)**2),
|
||||
),
|
||||
(
|
||||
(1, 0, 0),
|
||||
(Rational(1, 2), sqrt(3)*Rational(1, 2), 1),
|
||||
sqrt(1 + (Rational(1, 3)*pi)**2),
|
||||
),
|
||||
]
|
||||
)
|
||||
def test_static_pathway_on_cylinder_length(self, pA_vec, pB_vec, factor):
|
||||
pA_vec = self._expand_pos_to_vec(pA_vec, self.N)
|
||||
pB_vec = self._expand_pos_to_vec(pB_vec, self.N)
|
||||
self.pA.set_pos(self.pO, self.r*pA_vec)
|
||||
self.pB.set_pos(self.pO, self.r*pB_vec)
|
||||
pathway = WrappingPathway(self.pA, self.pB, self.cylinder)
|
||||
expected = factor*sqrt(self.r**2)
|
||||
assert simplify(pathway.length - expected) == 0
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
'pA_vec, pB_vec',
|
||||
[
|
||||
((1, 0, 0), (0, 1, 0)),
|
||||
((0, 1, 0), (sqrt(2)*Rational(1, 2), -sqrt(2)*Rational(1, 2), 0)),
|
||||
((1, 0, 0), (Rational(1, 2), sqrt(3)*Rational(1, 2), 0)),
|
||||
]
|
||||
)
|
||||
def test_static_pathway_on_sphere_extension_velocity(self, pA_vec, pB_vec):
|
||||
pA_vec = self._expand_pos_to_vec(pA_vec, self.N)
|
||||
pB_vec = self._expand_pos_to_vec(pB_vec, self.N)
|
||||
self.pA.set_pos(self.pO, self.r*pA_vec)
|
||||
self.pB.set_pos(self.pO, self.r*pB_vec)
|
||||
pathway = WrappingPathway(self.pA, self.pB, self.sphere)
|
||||
assert pathway.extension_velocity == 0
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
'pA_vec, pB_vec',
|
||||
[
|
||||
((1, 0, 0), (0, 1, 0)),
|
||||
((1, 0, 0), (-1, 0, 0)),
|
||||
((-1, 0, 0), (1, 0, 0)),
|
||||
((0, 1, 0), (sqrt(2)/2, -sqrt(2)/2, 0)),
|
||||
((1, 0, 0), (Rational(1, 2), sqrt(3)/2, 0)),
|
||||
((0, 1, 0), (sqrt(2)*Rational(1, 2), -sqrt(2)/2, 1)),
|
||||
((1, 0, 0), (Rational(1, 2), sqrt(3)/2, 1)),
|
||||
]
|
||||
)
|
||||
def test_static_pathway_on_cylinder_extension_velocity(self, pA_vec, pB_vec):
|
||||
pA_vec = self._expand_pos_to_vec(pA_vec, self.N)
|
||||
pB_vec = self._expand_pos_to_vec(pB_vec, self.N)
|
||||
self.pA.set_pos(self.pO, self.r*pA_vec)
|
||||
self.pB.set_pos(self.pO, self.r*pB_vec)
|
||||
pathway = WrappingPathway(self.pA, self.pB, self.cylinder)
|
||||
assert pathway.extension_velocity == 0
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
'pA_vec, pB_vec, pA_vec_expected, pB_vec_expected, pO_vec_expected',
|
||||
(
|
||||
((1, 0, 0), (0, 1, 0), (0, 1, 0), (1, 0, 0), (-1, -1, 0)),
|
||||
(
|
||||
(0, 1, 0),
|
||||
(sqrt(2)/2, -sqrt(2)/2, 0),
|
||||
(1, 0, 0),
|
||||
(sqrt(2)/2, sqrt(2)/2, 0),
|
||||
(-1 - sqrt(2)/2, -sqrt(2)/2, 0)
|
||||
),
|
||||
(
|
||||
(1, 0, 0),
|
||||
(Rational(1, 2), sqrt(3)/2, 0),
|
||||
(0, 1, 0),
|
||||
(sqrt(3)/2, -Rational(1, 2), 0),
|
||||
(-sqrt(3)/2, Rational(1, 2) - 1, 0),
|
||||
),
|
||||
)
|
||||
)
|
||||
def test_static_pathway_on_sphere_to_loads(
|
||||
self,
|
||||
pA_vec,
|
||||
pB_vec,
|
||||
pA_vec_expected,
|
||||
pB_vec_expected,
|
||||
pO_vec_expected,
|
||||
):
|
||||
pA_vec = self._expand_pos_to_vec(pA_vec, self.N)
|
||||
pB_vec = self._expand_pos_to_vec(pB_vec, self.N)
|
||||
self.pA.set_pos(self.pO, self.r*pA_vec)
|
||||
self.pB.set_pos(self.pO, self.r*pB_vec)
|
||||
pathway = WrappingPathway(self.pA, self.pB, self.sphere)
|
||||
|
||||
pA_vec_expected = sum(
|
||||
mag*unit for (mag, unit) in zip(pA_vec_expected, self.N)
|
||||
)
|
||||
pB_vec_expected = sum(
|
||||
mag*unit for (mag, unit) in zip(pB_vec_expected, self.N)
|
||||
)
|
||||
pO_vec_expected = sum(
|
||||
mag*unit for (mag, unit) in zip(pO_vec_expected, self.N)
|
||||
)
|
||||
expected = [
|
||||
Force(self.pA, self.F*(self.r**3/sqrt(self.r**6))*pA_vec_expected),
|
||||
Force(self.pB, self.F*(self.r**3/sqrt(self.r**6))*pB_vec_expected),
|
||||
Force(self.pO, self.F*(self.r**3/sqrt(self.r**6))*pO_vec_expected),
|
||||
]
|
||||
assert pathway.to_loads(self.F) == expected
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
'pA_vec, pB_vec, pA_vec_expected, pB_vec_expected, pO_vec_expected',
|
||||
(
|
||||
((1, 0, 0), (0, 1, 0), (0, 1, 0), (1, 0, 0), (-1, -1, 0)),
|
||||
((1, 0, 0), (-1, 0, 0), (0, 1, 0), (0, 1, 0), (0, -2, 0)),
|
||||
((-1, 0, 0), (1, 0, 0), (0, -1, 0), (0, -1, 0), (0, 2, 0)),
|
||||
(
|
||||
(0, 1, 0),
|
||||
(sqrt(2)/2, -sqrt(2)/2, 0),
|
||||
(-1, 0, 0),
|
||||
(-sqrt(2)/2, -sqrt(2)/2, 0),
|
||||
(1 + sqrt(2)/2, sqrt(2)/2, 0)
|
||||
),
|
||||
(
|
||||
(1, 0, 0),
|
||||
(Rational(1, 2), sqrt(3)/2, 0),
|
||||
(0, 1, 0),
|
||||
(sqrt(3)/2, -Rational(1, 2), 0),
|
||||
(-sqrt(3)/2, Rational(1, 2) - 1, 0),
|
||||
),
|
||||
(
|
||||
(1, 0, 0),
|
||||
(sqrt(2)/2, sqrt(2)/2, 0),
|
||||
(0, 1, 0),
|
||||
(sqrt(2)/2, -sqrt(2)/2, 0),
|
||||
(-sqrt(2)/2, sqrt(2)/2 - 1, 0),
|
||||
),
|
||||
((0, 1, 0), (0, 1, 1), (0, 0, 1), (0, 0, -1), (0, 0, 0)),
|
||||
(
|
||||
(0, 1, 0),
|
||||
(sqrt(2)/2, -sqrt(2)/2, 1),
|
||||
(-5*pi/sqrt(16 + 25*pi**2), 0, 4/sqrt(16 + 25*pi**2)),
|
||||
(
|
||||
-5*sqrt(2)*pi/(2*sqrt(16 + 25*pi**2)),
|
||||
-5*sqrt(2)*pi/(2*sqrt(16 + 25*pi**2)),
|
||||
-4/sqrt(16 + 25*pi**2),
|
||||
),
|
||||
(
|
||||
5*(sqrt(2) + 2)*pi/(2*sqrt(16 + 25*pi**2)),
|
||||
5*sqrt(2)*pi/(2*sqrt(16 + 25*pi**2)),
|
||||
0,
|
||||
),
|
||||
),
|
||||
)
|
||||
)
|
||||
def test_static_pathway_on_cylinder_to_loads(
|
||||
self,
|
||||
pA_vec,
|
||||
pB_vec,
|
||||
pA_vec_expected,
|
||||
pB_vec_expected,
|
||||
pO_vec_expected,
|
||||
):
|
||||
pA_vec = self._expand_pos_to_vec(pA_vec, self.N)
|
||||
pB_vec = self._expand_pos_to_vec(pB_vec, self.N)
|
||||
self.pA.set_pos(self.pO, self.r*pA_vec)
|
||||
self.pB.set_pos(self.pO, self.r*pB_vec)
|
||||
pathway = WrappingPathway(self.pA, self.pB, self.cylinder)
|
||||
|
||||
pA_force_expected = self.F*self._expand_pos_to_vec(pA_vec_expected,
|
||||
self.N)
|
||||
pB_force_expected = self.F*self._expand_pos_to_vec(pB_vec_expected,
|
||||
self.N)
|
||||
pO_force_expected = self.F*self._expand_pos_to_vec(pO_vec_expected,
|
||||
self.N)
|
||||
expected = [
|
||||
Force(self.pA, pA_force_expected),
|
||||
Force(self.pB, pB_force_expected),
|
||||
Force(self.pO, pO_force_expected),
|
||||
]
|
||||
assert _simplify_loads(pathway.to_loads(self.F)) == expected
|
||||
|
||||
def test_2D_pathway_on_cylinder_length(self):
|
||||
q = dynamicsymbols('q')
|
||||
pA_pos = self.r*self.N.x
|
||||
pB_pos = self.r*(cos(q)*self.N.x + sin(q)*self.N.y)
|
||||
self.pA.set_pos(self.pO, pA_pos)
|
||||
self.pB.set_pos(self.pO, pB_pos)
|
||||
expected = self.r*sqrt(q**2)
|
||||
assert simplify(self.pathway.length - expected) == 0
|
||||
|
||||
def test_2D_pathway_on_cylinder_extension_velocity(self):
|
||||
q = dynamicsymbols('q')
|
||||
qd = dynamicsymbols('q', 1)
|
||||
pA_pos = self.r*self.N.x
|
||||
pB_pos = self.r*(cos(q)*self.N.x + sin(q)*self.N.y)
|
||||
self.pA.set_pos(self.pO, pA_pos)
|
||||
self.pB.set_pos(self.pO, pB_pos)
|
||||
expected = self.r*(sqrt(q**2)/q)*qd
|
||||
assert simplify(self.pathway.extension_velocity - expected) == 0
|
||||
|
||||
def test_2D_pathway_on_cylinder_to_loads(self):
|
||||
q = dynamicsymbols('q')
|
||||
pA_pos = self.r*self.N.x
|
||||
pB_pos = self.r*(cos(q)*self.N.x + sin(q)*self.N.y)
|
||||
self.pA.set_pos(self.pO, pA_pos)
|
||||
self.pB.set_pos(self.pO, pB_pos)
|
||||
|
||||
pA_force = self.F*self.N.y
|
||||
pB_force = self.F*(sin(q)*self.N.x - cos(q)*self.N.y)
|
||||
pO_force = self.F*(-sin(q)*self.N.x + (cos(q) - 1)*self.N.y)
|
||||
expected = [
|
||||
Force(self.pA, pA_force),
|
||||
Force(self.pB, pB_force),
|
||||
Force(self.pO, pO_force),
|
||||
]
|
||||
|
||||
loads = _simplify_loads(self.pathway.to_loads(self.F))
|
||||
assert loads == expected
|
||||
+184
@@ -0,0 +1,184 @@
|
||||
from sympy.physics.mechanics import Point, ReferenceFrame, Dyadic, RigidBody
|
||||
from sympy.physics.mechanics import dynamicsymbols, outer, inertia, Inertia
|
||||
from sympy.physics.mechanics import inertia_of_point_mass
|
||||
from sympy import expand, zeros, simplify, symbols
|
||||
from sympy.testing.pytest import raises, warns_deprecated_sympy
|
||||
|
||||
|
||||
def test_rigidbody_default():
|
||||
# Test default
|
||||
b = RigidBody('B')
|
||||
I = inertia(b.frame, *symbols('B_ixx B_iyy B_izz B_ixy B_iyz B_izx'))
|
||||
assert b.name == 'B'
|
||||
assert b.mass == symbols('B_mass')
|
||||
assert b.masscenter.name == 'B_masscenter'
|
||||
assert b.inertia == (I, b.masscenter)
|
||||
assert b.central_inertia == I
|
||||
assert b.frame.name == 'B_frame'
|
||||
assert b.__str__() == 'B'
|
||||
assert b.__repr__() == (
|
||||
"RigidBody('B', masscenter=B_masscenter, frame=B_frame, mass=B_mass, "
|
||||
"inertia=Inertia(dyadic=B_ixx*(B_frame.x|B_frame.x) + "
|
||||
"B_ixy*(B_frame.x|B_frame.y) + B_izx*(B_frame.x|B_frame.z) + "
|
||||
"B_ixy*(B_frame.y|B_frame.x) + B_iyy*(B_frame.y|B_frame.y) + "
|
||||
"B_iyz*(B_frame.y|B_frame.z) + B_izx*(B_frame.z|B_frame.x) + "
|
||||
"B_iyz*(B_frame.z|B_frame.y) + B_izz*(B_frame.z|B_frame.z), "
|
||||
"point=B_masscenter))")
|
||||
|
||||
|
||||
def test_rigidbody():
|
||||
m, m2, v1, v2, v3, omega = symbols('m m2 v1 v2 v3 omega')
|
||||
A = ReferenceFrame('A')
|
||||
A2 = ReferenceFrame('A2')
|
||||
P = Point('P')
|
||||
P2 = Point('P2')
|
||||
I = Dyadic(0)
|
||||
I2 = Dyadic(0)
|
||||
B = RigidBody('B', P, A, m, (I, P))
|
||||
assert B.mass == m
|
||||
assert B.frame == A
|
||||
assert B.masscenter == P
|
||||
assert B.inertia == (I, B.masscenter)
|
||||
|
||||
B.mass = m2
|
||||
B.frame = A2
|
||||
B.masscenter = P2
|
||||
B.inertia = (I2, B.masscenter)
|
||||
raises(TypeError, lambda: RigidBody(P, P, A, m, (I, P)))
|
||||
raises(TypeError, lambda: RigidBody('B', P, P, m, (I, P)))
|
||||
raises(TypeError, lambda: RigidBody('B', P, A, m, (P, P)))
|
||||
raises(TypeError, lambda: RigidBody('B', P, A, m, (I, I)))
|
||||
assert B.__str__() == 'B'
|
||||
assert B.mass == m2
|
||||
assert B.frame == A2
|
||||
assert B.masscenter == P2
|
||||
assert B.inertia == (I2, B.masscenter)
|
||||
assert isinstance(B.inertia, Inertia)
|
||||
|
||||
# Testing linear momentum function assuming A2 is the inertial frame
|
||||
N = ReferenceFrame('N')
|
||||
P2.set_vel(N, v1 * N.x + v2 * N.y + v3 * N.z)
|
||||
assert B.linear_momentum(N) == m2 * (v1 * N.x + v2 * N.y + v3 * N.z)
|
||||
|
||||
|
||||
def test_rigidbody2():
|
||||
M, v, r, omega, g, h = dynamicsymbols('M v r omega g h')
|
||||
N = ReferenceFrame('N')
|
||||
b = ReferenceFrame('b')
|
||||
b.set_ang_vel(N, omega * b.x)
|
||||
P = Point('P')
|
||||
I = outer(b.x, b.x)
|
||||
Inertia_tuple = (I, P)
|
||||
B = RigidBody('B', P, b, M, Inertia_tuple)
|
||||
P.set_vel(N, v * b.x)
|
||||
assert B.angular_momentum(P, N) == omega * b.x
|
||||
O = Point('O')
|
||||
O.set_vel(N, v * b.x)
|
||||
P.set_pos(O, r * b.y)
|
||||
assert B.angular_momentum(O, N) == omega * b.x - M*v*r*b.z
|
||||
B.potential_energy = M * g * h
|
||||
assert B.potential_energy == M * g * h
|
||||
assert expand(2 * B.kinetic_energy(N)) == omega**2 + M * v**2
|
||||
|
||||
|
||||
def test_rigidbody3():
|
||||
q1, q2, q3, q4 = dynamicsymbols('q1:5')
|
||||
p1, p2, p3 = symbols('p1:4')
|
||||
m = symbols('m')
|
||||
|
||||
A = ReferenceFrame('A')
|
||||
B = A.orientnew('B', 'axis', [q1, A.x])
|
||||
O = Point('O')
|
||||
O.set_vel(A, q2*A.x + q3*A.y + q4*A.z)
|
||||
P = O.locatenew('P', p1*B.x + p2*B.y + p3*B.z)
|
||||
P.v2pt_theory(O, A, B)
|
||||
I = outer(B.x, B.x)
|
||||
|
||||
rb1 = RigidBody('rb1', P, B, m, (I, P))
|
||||
# I_S/O = I_S/S* + I_S*/O
|
||||
rb2 = RigidBody('rb2', P, B, m,
|
||||
(I + inertia_of_point_mass(m, P.pos_from(O), B), O))
|
||||
|
||||
assert rb1.central_inertia == rb2.central_inertia
|
||||
assert rb1.angular_momentum(O, A) == rb2.angular_momentum(O, A)
|
||||
|
||||
|
||||
def test_pendulum_angular_momentum():
|
||||
"""Consider a pendulum of length OA = 2a, of mass m as a rigid body of
|
||||
center of mass G (OG = a) which turn around (O,z). The angle between the
|
||||
reference frame R and the rod is q. The inertia of the body is I =
|
||||
(G,0,ma^2/3,ma^2/3). """
|
||||
|
||||
m, a = symbols('m, a')
|
||||
q = dynamicsymbols('q')
|
||||
|
||||
R = ReferenceFrame('R')
|
||||
R1 = R.orientnew('R1', 'Axis', [q, R.z])
|
||||
R1.set_ang_vel(R, q.diff() * R.z)
|
||||
|
||||
I = inertia(R1, 0, m * a**2 / 3, m * a**2 / 3)
|
||||
|
||||
O = Point('O')
|
||||
|
||||
A = O.locatenew('A', 2*a * R1.x)
|
||||
G = O.locatenew('G', a * R1.x)
|
||||
|
||||
S = RigidBody('S', G, R1, m, (I, G))
|
||||
|
||||
O.set_vel(R, 0)
|
||||
A.v2pt_theory(O, R, R1)
|
||||
G.v2pt_theory(O, R, R1)
|
||||
|
||||
assert (4 * m * a**2 / 3 * q.diff() * R.z -
|
||||
S.angular_momentum(O, R).express(R)) == 0
|
||||
|
||||
|
||||
def test_rigidbody_inertia():
|
||||
N = ReferenceFrame('N')
|
||||
m, Ix, Iy, Iz, a, b = symbols('m, I_x, I_y, I_z, a, b')
|
||||
Io = inertia(N, Ix, Iy, Iz)
|
||||
o = Point('o')
|
||||
p = o.locatenew('p', a * N.x + b * N.y)
|
||||
R = RigidBody('R', o, N, m, (Io, p))
|
||||
I_check = inertia(N, Ix - b ** 2 * m, Iy - a ** 2 * m,
|
||||
Iz - m * (a ** 2 + b ** 2), m * a * b)
|
||||
assert isinstance(R.inertia, Inertia)
|
||||
assert R.inertia == (Io, p)
|
||||
assert R.central_inertia == I_check
|
||||
R.central_inertia = Io
|
||||
assert R.inertia == (Io, o)
|
||||
assert R.central_inertia == Io
|
||||
R.inertia = (Io, p)
|
||||
assert R.inertia == (Io, p)
|
||||
assert R.central_inertia == I_check
|
||||
# parse Inertia object
|
||||
R.inertia = Inertia(Io, o)
|
||||
assert R.inertia == (Io, o)
|
||||
|
||||
|
||||
def test_parallel_axis():
|
||||
N = ReferenceFrame('N')
|
||||
m, Ix, Iy, Iz, a, b = symbols('m, I_x, I_y, I_z, a, b')
|
||||
Io = inertia(N, Ix, Iy, Iz)
|
||||
o = Point('o')
|
||||
p = o.locatenew('p', a * N.x + b * N.y)
|
||||
R = RigidBody('R', o, N, m, (Io, o))
|
||||
Ip = R.parallel_axis(p)
|
||||
Ip_expected = inertia(N, Ix + m * b**2, Iy + m * a**2,
|
||||
Iz + m * (a**2 + b**2), ixy=-m * a * b)
|
||||
assert Ip == Ip_expected
|
||||
# Reference frame from which the parallel axis is viewed should not matter
|
||||
A = ReferenceFrame('A')
|
||||
A.orient_axis(N, N.z, 1)
|
||||
assert simplify(
|
||||
(R.parallel_axis(p, A) - Ip_expected).to_matrix(A)) == zeros(3, 3)
|
||||
|
||||
|
||||
def test_deprecated_set_potential_energy():
|
||||
m, g, h = symbols('m g h')
|
||||
A = ReferenceFrame('A')
|
||||
P = Point('P')
|
||||
I = Dyadic(0)
|
||||
B = RigidBody('B', P, A, m, (I, P))
|
||||
with warns_deprecated_sympy():
|
||||
B.set_potential_energy(m*g*h)
|
||||
+245
@@ -0,0 +1,245 @@
|
||||
from sympy import symbols, Matrix, atan, zeros
|
||||
from sympy.simplify.simplify import simplify
|
||||
from sympy.physics.mechanics import (dynamicsymbols, Particle, Point,
|
||||
ReferenceFrame, SymbolicSystem)
|
||||
from sympy.testing.pytest import raises
|
||||
|
||||
# This class is going to be tested using a simple pendulum set up in x and y
|
||||
# coordinates
|
||||
x, y, u, v, lam = dynamicsymbols('x y u v lambda')
|
||||
m, l, g = symbols('m l g')
|
||||
|
||||
# Set up the different forms the equations can take
|
||||
# [1] Explicit form where the kinematics and dynamics are combined
|
||||
# x' = F(x, t, r, p)
|
||||
#
|
||||
# [2] Implicit form where the kinematics and dynamics are combined
|
||||
# M(x, p) x' = F(x, t, r, p)
|
||||
#
|
||||
# [3] Implicit form where the kinematics and dynamics are separate
|
||||
# M(q, p) u' = F(q, u, t, r, p)
|
||||
# q' = G(q, u, t, r, p)
|
||||
dyn_implicit_mat = Matrix([[1, 0, -x/m],
|
||||
[0, 1, -y/m],
|
||||
[0, 0, l**2/m]])
|
||||
|
||||
dyn_implicit_rhs = Matrix([0, 0, u**2 + v**2 - g*y])
|
||||
|
||||
comb_implicit_mat = Matrix([[1, 0, 0, 0, 0],
|
||||
[0, 1, 0, 0, 0],
|
||||
[0, 0, 1, 0, -x/m],
|
||||
[0, 0, 0, 1, -y/m],
|
||||
[0, 0, 0, 0, l**2/m]])
|
||||
|
||||
comb_implicit_rhs = Matrix([u, v, 0, 0, u**2 + v**2 - g*y])
|
||||
|
||||
kin_explicit_rhs = Matrix([u, v])
|
||||
|
||||
comb_explicit_rhs = comb_implicit_mat.LUsolve(comb_implicit_rhs)
|
||||
|
||||
# Set up a body and load to pass into the system
|
||||
theta = atan(x/y)
|
||||
N = ReferenceFrame('N')
|
||||
A = N.orientnew('A', 'Axis', [theta, N.z])
|
||||
O = Point('O')
|
||||
P = O.locatenew('P', l * A.x)
|
||||
|
||||
Pa = Particle('Pa', P, m)
|
||||
|
||||
bodies = [Pa]
|
||||
loads = [(P, g * m * N.x)]
|
||||
|
||||
# Set up some output equations to be given to SymbolicSystem
|
||||
# Change to make these fit the pendulum
|
||||
PE = symbols("PE")
|
||||
out_eqns = {PE: m*g*(l+y)}
|
||||
|
||||
# Set up remaining arguments that can be passed to SymbolicSystem
|
||||
alg_con = [2]
|
||||
alg_con_full = [4]
|
||||
coordinates = (x, y, lam)
|
||||
speeds = (u, v)
|
||||
states = (x, y, u, v, lam)
|
||||
coord_idxs = (0, 1)
|
||||
speed_idxs = (2, 3)
|
||||
|
||||
|
||||
def test_form_1():
|
||||
symsystem1 = SymbolicSystem(states, comb_explicit_rhs,
|
||||
alg_con=alg_con_full, output_eqns=out_eqns,
|
||||
coord_idxs=coord_idxs, speed_idxs=speed_idxs,
|
||||
bodies=bodies, loads=loads)
|
||||
|
||||
assert symsystem1.coordinates == Matrix([x, y])
|
||||
assert symsystem1.speeds == Matrix([u, v])
|
||||
assert symsystem1.states == Matrix([x, y, u, v, lam])
|
||||
|
||||
assert symsystem1.alg_con == [4]
|
||||
|
||||
inter = comb_explicit_rhs
|
||||
assert simplify(symsystem1.comb_explicit_rhs - inter) == zeros(5, 1)
|
||||
|
||||
assert set(symsystem1.dynamic_symbols()) == {y, v, lam, u, x}
|
||||
assert type(symsystem1.dynamic_symbols()) == tuple
|
||||
assert set(symsystem1.constant_symbols()) == {l, g, m}
|
||||
assert type(symsystem1.constant_symbols()) == tuple
|
||||
|
||||
assert symsystem1.output_eqns == out_eqns
|
||||
|
||||
assert symsystem1.bodies == (Pa,)
|
||||
assert symsystem1.loads == ((P, g * m * N.x),)
|
||||
|
||||
|
||||
def test_form_2():
|
||||
symsystem2 = SymbolicSystem(coordinates, comb_implicit_rhs, speeds=speeds,
|
||||
mass_matrix=comb_implicit_mat,
|
||||
alg_con=alg_con_full, output_eqns=out_eqns,
|
||||
bodies=bodies, loads=loads)
|
||||
|
||||
assert symsystem2.coordinates == Matrix([x, y, lam])
|
||||
assert symsystem2.speeds == Matrix([u, v])
|
||||
assert symsystem2.states == Matrix([x, y, lam, u, v])
|
||||
|
||||
assert symsystem2.alg_con == [4]
|
||||
|
||||
inter = comb_implicit_rhs
|
||||
assert simplify(symsystem2.comb_implicit_rhs - inter) == zeros(5, 1)
|
||||
assert simplify(symsystem2.comb_implicit_mat-comb_implicit_mat) == zeros(5)
|
||||
|
||||
assert set(symsystem2.dynamic_symbols()) == {y, v, lam, u, x}
|
||||
assert type(symsystem2.dynamic_symbols()) == tuple
|
||||
assert set(symsystem2.constant_symbols()) == {l, g, m}
|
||||
assert type(symsystem2.constant_symbols()) == tuple
|
||||
|
||||
inter = comb_explicit_rhs
|
||||
symsystem2.compute_explicit_form()
|
||||
assert simplify(symsystem2.comb_explicit_rhs - inter) == zeros(5, 1)
|
||||
|
||||
|
||||
assert symsystem2.output_eqns == out_eqns
|
||||
|
||||
assert symsystem2.bodies == (Pa,)
|
||||
assert symsystem2.loads == ((P, g * m * N.x),)
|
||||
|
||||
|
||||
def test_form_3():
|
||||
symsystem3 = SymbolicSystem(states, dyn_implicit_rhs,
|
||||
mass_matrix=dyn_implicit_mat,
|
||||
coordinate_derivatives=kin_explicit_rhs,
|
||||
alg_con=alg_con, coord_idxs=coord_idxs,
|
||||
speed_idxs=speed_idxs, bodies=bodies,
|
||||
loads=loads)
|
||||
|
||||
assert symsystem3.coordinates == Matrix([x, y])
|
||||
assert symsystem3.speeds == Matrix([u, v])
|
||||
assert symsystem3.states == Matrix([x, y, u, v, lam])
|
||||
|
||||
assert symsystem3.alg_con == [4]
|
||||
|
||||
inter1 = kin_explicit_rhs
|
||||
inter2 = dyn_implicit_rhs
|
||||
assert simplify(symsystem3.kin_explicit_rhs - inter1) == zeros(2, 1)
|
||||
assert simplify(symsystem3.dyn_implicit_mat - dyn_implicit_mat) == zeros(3)
|
||||
assert simplify(symsystem3.dyn_implicit_rhs - inter2) == zeros(3, 1)
|
||||
|
||||
inter = comb_implicit_rhs
|
||||
assert simplify(symsystem3.comb_implicit_rhs - inter) == zeros(5, 1)
|
||||
assert simplify(symsystem3.comb_implicit_mat-comb_implicit_mat) == zeros(5)
|
||||
|
||||
inter = comb_explicit_rhs
|
||||
symsystem3.compute_explicit_form()
|
||||
assert simplify(symsystem3.comb_explicit_rhs - inter) == zeros(5, 1)
|
||||
|
||||
assert set(symsystem3.dynamic_symbols()) == {y, v, lam, u, x}
|
||||
assert type(symsystem3.dynamic_symbols()) == tuple
|
||||
assert set(symsystem3.constant_symbols()) == {l, g, m}
|
||||
assert type(symsystem3.constant_symbols()) == tuple
|
||||
|
||||
assert symsystem3.output_eqns == {}
|
||||
|
||||
assert symsystem3.bodies == (Pa,)
|
||||
assert symsystem3.loads == ((P, g * m * N.x),)
|
||||
|
||||
|
||||
def test_property_attributes():
|
||||
symsystem = SymbolicSystem(states, comb_explicit_rhs,
|
||||
alg_con=alg_con_full, output_eqns=out_eqns,
|
||||
coord_idxs=coord_idxs, speed_idxs=speed_idxs,
|
||||
bodies=bodies, loads=loads)
|
||||
|
||||
with raises(AttributeError):
|
||||
symsystem.bodies = 42
|
||||
with raises(AttributeError):
|
||||
symsystem.coordinates = 42
|
||||
with raises(AttributeError):
|
||||
symsystem.dyn_implicit_rhs = 42
|
||||
with raises(AttributeError):
|
||||
symsystem.comb_implicit_rhs = 42
|
||||
with raises(AttributeError):
|
||||
symsystem.loads = 42
|
||||
with raises(AttributeError):
|
||||
symsystem.dyn_implicit_mat = 42
|
||||
with raises(AttributeError):
|
||||
symsystem.comb_implicit_mat = 42
|
||||
with raises(AttributeError):
|
||||
symsystem.kin_explicit_rhs = 42
|
||||
with raises(AttributeError):
|
||||
symsystem.comb_explicit_rhs = 42
|
||||
with raises(AttributeError):
|
||||
symsystem.speeds = 42
|
||||
with raises(AttributeError):
|
||||
symsystem.states = 42
|
||||
with raises(AttributeError):
|
||||
symsystem.alg_con = 42
|
||||
|
||||
|
||||
def test_not_specified_errors():
|
||||
"""This test will cover errors that arise from trying to access attributes
|
||||
that were not specified upon object creation or were specified on creation
|
||||
and the user tries to recalculate them."""
|
||||
# Trying to access form 2 when form 1 given
|
||||
# Trying to access form 3 when form 2 given
|
||||
|
||||
symsystem1 = SymbolicSystem(states, comb_explicit_rhs)
|
||||
|
||||
with raises(AttributeError):
|
||||
symsystem1.comb_implicit_mat
|
||||
with raises(AttributeError):
|
||||
symsystem1.comb_implicit_rhs
|
||||
with raises(AttributeError):
|
||||
symsystem1.dyn_implicit_mat
|
||||
with raises(AttributeError):
|
||||
symsystem1.dyn_implicit_rhs
|
||||
with raises(AttributeError):
|
||||
symsystem1.kin_explicit_rhs
|
||||
with raises(AttributeError):
|
||||
symsystem1.compute_explicit_form()
|
||||
|
||||
symsystem2 = SymbolicSystem(coordinates, comb_implicit_rhs, speeds=speeds,
|
||||
mass_matrix=comb_implicit_mat)
|
||||
|
||||
with raises(AttributeError):
|
||||
symsystem2.dyn_implicit_mat
|
||||
with raises(AttributeError):
|
||||
symsystem2.dyn_implicit_rhs
|
||||
with raises(AttributeError):
|
||||
symsystem2.kin_explicit_rhs
|
||||
|
||||
# Attribute error when trying to access coordinates and speeds when only the
|
||||
# states were given.
|
||||
with raises(AttributeError):
|
||||
symsystem1.coordinates
|
||||
with raises(AttributeError):
|
||||
symsystem1.speeds
|
||||
|
||||
# Attribute error when trying to access bodies and loads when they are not
|
||||
# given
|
||||
with raises(AttributeError):
|
||||
symsystem1.bodies
|
||||
with raises(AttributeError):
|
||||
symsystem1.loads
|
||||
|
||||
# Attribute error when trying to access comb_explicit_rhs before it was
|
||||
# calculated
|
||||
with raises(AttributeError):
|
||||
symsystem2.comb_explicit_rhs
|
||||
+831
@@ -0,0 +1,831 @@
|
||||
import pytest
|
||||
|
||||
from sympy.core.symbol import symbols
|
||||
from sympy.core.sympify import sympify
|
||||
from sympy.functions.elementary.trigonometric import cos, sin
|
||||
from sympy.matrices.dense import eye, zeros
|
||||
from sympy.matrices.immutable import ImmutableMatrix
|
||||
from sympy.physics.mechanics import (
|
||||
Force, KanesMethod, LagrangesMethod, Particle, PinJoint, Point,
|
||||
PrismaticJoint, ReferenceFrame, RigidBody, Torque, TorqueActuator, System,
|
||||
dynamicsymbols)
|
||||
from sympy.simplify.simplify import simplify
|
||||
from sympy.solvers.solvers import solve
|
||||
|
||||
t = dynamicsymbols._t # type: ignore
|
||||
q = dynamicsymbols('q:6') # type: ignore
|
||||
qd = dynamicsymbols('q:6', 1) # type: ignore
|
||||
u = dynamicsymbols('u:6') # type: ignore
|
||||
ua = dynamicsymbols('ua:3') # type: ignore
|
||||
|
||||
|
||||
class TestSystemBase:
|
||||
@pytest.fixture()
|
||||
def _empty_system_setup(self):
|
||||
self.system = System(ReferenceFrame('frame'), Point('fixed_point'))
|
||||
|
||||
def _empty_system_check(self, exclude=()):
|
||||
matrices = ('q_ind', 'q_dep', 'q', 'u_ind', 'u_dep', 'u', 'u_aux',
|
||||
'kdes', 'holonomic_constraints', 'nonholonomic_constraints')
|
||||
tuples = ('loads', 'bodies', 'joints', 'actuators')
|
||||
for attr in matrices:
|
||||
if attr not in exclude:
|
||||
assert getattr(self.system, attr)[:] == []
|
||||
for attr in tuples:
|
||||
if attr not in exclude:
|
||||
assert getattr(self.system, attr) == ()
|
||||
if 'eom_method' not in exclude:
|
||||
assert self.system.eom_method is None
|
||||
|
||||
def _create_filled_system(self, with_speeds=True):
|
||||
self.system = System(ReferenceFrame('frame'), Point('fixed_point'))
|
||||
u = dynamicsymbols('u:6') if with_speeds else qd
|
||||
self.bodies = symbols('rb1:5', cls=RigidBody)
|
||||
self.joints = (
|
||||
PinJoint('J1', self.bodies[0], self.bodies[1], q[0], u[0]),
|
||||
PrismaticJoint('J2', self.bodies[1], self.bodies[2], q[1], u[1]),
|
||||
PinJoint('J3', self.bodies[2], self.bodies[3], q[2], u[2])
|
||||
)
|
||||
self.system.add_joints(*self.joints)
|
||||
self.system.add_coordinates(q[3], independent=[False])
|
||||
self.system.add_speeds(u[3], independent=False)
|
||||
if with_speeds:
|
||||
self.system.add_kdes(u[3] - qd[3])
|
||||
self.system.add_auxiliary_speeds(ua[0], ua[1])
|
||||
self.system.add_holonomic_constraints(q[2] - q[0] + q[1])
|
||||
self.system.add_nonholonomic_constraints(u[3] - qd[1] + u[2])
|
||||
self.system.u_ind = u[:2]
|
||||
self.system.u_dep = u[2:4]
|
||||
self.q_ind, self.q_dep = self.system.q_ind[:], self.system.q_dep[:]
|
||||
self.u_ind, self.u_dep = self.system.u_ind[:], self.system.u_dep[:]
|
||||
self.kdes = self.system.kdes[:]
|
||||
self.hc = self.system.holonomic_constraints[:]
|
||||
self.vc = self.system.velocity_constraints[:]
|
||||
self.nhc = self.system.nonholonomic_constraints[:]
|
||||
|
||||
@pytest.fixture()
|
||||
def _filled_system_setup(self):
|
||||
self._create_filled_system(with_speeds=True)
|
||||
|
||||
@pytest.fixture()
|
||||
def _filled_system_setup_no_speeds(self):
|
||||
self._create_filled_system(with_speeds=False)
|
||||
|
||||
def _filled_system_check(self, exclude=()):
|
||||
assert 'q_ind' in exclude or self.system.q_ind[:] == q[:3]
|
||||
assert 'q_dep' in exclude or self.system.q_dep[:] == [q[3]]
|
||||
assert 'q' in exclude or self.system.q[:] == q[:4]
|
||||
assert 'u_ind' in exclude or self.system.u_ind[:] == u[:2]
|
||||
assert 'u_dep' in exclude or self.system.u_dep[:] == u[2:4]
|
||||
assert 'u' in exclude or self.system.u[:] == u[:4]
|
||||
assert 'u_aux' in exclude or self.system.u_aux[:] == ua[:2]
|
||||
assert 'kdes' in exclude or self.system.kdes[:] == [
|
||||
ui - qdi for ui, qdi in zip(u[:4], qd[:4])]
|
||||
assert ('holonomic_constraints' in exclude or
|
||||
self.system.holonomic_constraints[:] == [q[2] - q[0] + q[1]])
|
||||
assert ('nonholonomic_constraints' in exclude or
|
||||
self.system.nonholonomic_constraints[:] == [u[3] - qd[1] + u[2]]
|
||||
)
|
||||
assert ('velocity_constraints' in exclude or
|
||||
self.system.velocity_constraints[:] == [
|
||||
qd[2] - qd[0] + qd[1], u[3] - qd[1] + u[2]])
|
||||
assert ('bodies' in exclude or
|
||||
self.system.bodies == tuple(self.bodies))
|
||||
assert ('joints' in exclude or
|
||||
self.system.joints == tuple(self.joints))
|
||||
|
||||
@pytest.fixture()
|
||||
def _moving_point_mass(self, _empty_system_setup):
|
||||
self.system.q_ind = q[0]
|
||||
self.system.u_ind = u[0]
|
||||
self.system.kdes = u[0] - q[0].diff(t)
|
||||
p = Particle('p', mass=symbols('m'))
|
||||
self.system.add_bodies(p)
|
||||
p.masscenter.set_pos(self.system.fixed_point, q[0] * self.system.x)
|
||||
|
||||
|
||||
class TestSystem(TestSystemBase):
|
||||
def test_empty_system(self, _empty_system_setup):
|
||||
self._empty_system_check()
|
||||
self.system.validate_system()
|
||||
|
||||
def test_filled_system(self, _filled_system_setup):
|
||||
self._filled_system_check()
|
||||
self.system.validate_system()
|
||||
|
||||
@pytest.mark.parametrize('frame', [None, ReferenceFrame('frame')])
|
||||
@pytest.mark.parametrize('fixed_point', [None, Point('fixed_point')])
|
||||
def test_init(self, frame, fixed_point):
|
||||
if fixed_point is None and frame is None:
|
||||
self.system = System()
|
||||
else:
|
||||
self.system = System(frame, fixed_point)
|
||||
if fixed_point is None:
|
||||
assert self.system.fixed_point.name == 'inertial_point'
|
||||
else:
|
||||
assert self.system.fixed_point == fixed_point
|
||||
if frame is None:
|
||||
assert self.system.frame.name == 'inertial_frame'
|
||||
else:
|
||||
assert self.system.frame == frame
|
||||
self._empty_system_check()
|
||||
assert isinstance(self.system.q_ind, ImmutableMatrix)
|
||||
assert isinstance(self.system.q_dep, ImmutableMatrix)
|
||||
assert isinstance(self.system.q, ImmutableMatrix)
|
||||
assert isinstance(self.system.u_ind, ImmutableMatrix)
|
||||
assert isinstance(self.system.u_dep, ImmutableMatrix)
|
||||
assert isinstance(self.system.u, ImmutableMatrix)
|
||||
assert isinstance(self.system.kdes, ImmutableMatrix)
|
||||
assert isinstance(self.system.holonomic_constraints, ImmutableMatrix)
|
||||
assert isinstance(self.system.nonholonomic_constraints, ImmutableMatrix)
|
||||
|
||||
def test_from_newtonian_rigid_body(self):
|
||||
rb = RigidBody('body')
|
||||
self.system = System.from_newtonian(rb)
|
||||
assert self.system.fixed_point == rb.masscenter
|
||||
assert self.system.frame == rb.frame
|
||||
self._empty_system_check(exclude=('bodies',))
|
||||
self.system.bodies = (rb,)
|
||||
|
||||
def test_from_newtonian_particle(self):
|
||||
pt = Particle('particle')
|
||||
with pytest.raises(TypeError):
|
||||
System.from_newtonian(pt)
|
||||
|
||||
@pytest.mark.parametrize('args, kwargs, exp_q_ind, exp_q_dep, exp_q', [
|
||||
(q[:3], {}, q[:3], [], q[:3]),
|
||||
(q[:3], {'independent': True}, q[:3], [], q[:3]),
|
||||
(q[:3], {'independent': False}, [], q[:3], q[:3]),
|
||||
(q[:3], {'independent': [True, False, True]}, [q[0], q[2]], [q[1]],
|
||||
[q[0], q[2], q[1]]),
|
||||
])
|
||||
def test_coordinates(self, _empty_system_setup, args, kwargs,
|
||||
exp_q_ind, exp_q_dep, exp_q):
|
||||
# Test add_coordinates
|
||||
self.system.add_coordinates(*args, **kwargs)
|
||||
assert self.system.q_ind[:] == exp_q_ind
|
||||
assert self.system.q_dep[:] == exp_q_dep
|
||||
assert self.system.q[:] == exp_q
|
||||
self._empty_system_check(exclude=('q_ind', 'q_dep', 'q'))
|
||||
# Test setter for q_ind and q_dep
|
||||
self.system.q_ind = exp_q_ind
|
||||
self.system.q_dep = exp_q_dep
|
||||
assert self.system.q_ind[:] == exp_q_ind
|
||||
assert self.system.q_dep[:] == exp_q_dep
|
||||
assert self.system.q[:] == exp_q
|
||||
self._empty_system_check(exclude=('q_ind', 'q_dep', 'q'))
|
||||
|
||||
@pytest.mark.parametrize('func', ['add_coordinates', 'add_speeds'])
|
||||
@pytest.mark.parametrize('args, kwargs', [
|
||||
((q[0], q[5]), {}),
|
||||
((u[0], u[5]), {}),
|
||||
((q[0],), {'independent': False}),
|
||||
((u[0],), {'independent': False}),
|
||||
((u[0], q[5]), {}),
|
||||
((symbols('a'), q[5]), {}),
|
||||
])
|
||||
def test_coordinates_speeds_invalid(self, _filled_system_setup, func, args,
|
||||
kwargs):
|
||||
with pytest.raises(ValueError):
|
||||
getattr(self.system, func)(*args, **kwargs)
|
||||
self._filled_system_check()
|
||||
|
||||
@pytest.mark.parametrize('args, kwargs, exp_u_ind, exp_u_dep, exp_u', [
|
||||
(u[:3], {}, u[:3], [], u[:3]),
|
||||
(u[:3], {'independent': True}, u[:3], [], u[:3]),
|
||||
(u[:3], {'independent': False}, [], u[:3], u[:3]),
|
||||
(u[:3], {'independent': [True, False, True]}, [u[0], u[2]], [u[1]],
|
||||
[u[0], u[2], u[1]]),
|
||||
])
|
||||
def test_speeds(self, _empty_system_setup, args, kwargs, exp_u_ind,
|
||||
exp_u_dep, exp_u):
|
||||
# Test add_speeds
|
||||
self.system.add_speeds(*args, **kwargs)
|
||||
assert self.system.u_ind[:] == exp_u_ind
|
||||
assert self.system.u_dep[:] == exp_u_dep
|
||||
assert self.system.u[:] == exp_u
|
||||
self._empty_system_check(exclude=('u_ind', 'u_dep', 'u'))
|
||||
# Test setter for u_ind and u_dep
|
||||
self.system.u_ind = exp_u_ind
|
||||
self.system.u_dep = exp_u_dep
|
||||
assert self.system.u_ind[:] == exp_u_ind
|
||||
assert self.system.u_dep[:] == exp_u_dep
|
||||
assert self.system.u[:] == exp_u
|
||||
self._empty_system_check(exclude=('u_ind', 'u_dep', 'u'))
|
||||
|
||||
@pytest.mark.parametrize('args, kwargs, exp_u_aux', [
|
||||
(ua[:3], {}, ua[:3]),
|
||||
])
|
||||
def test_auxiliary_speeds(self, _empty_system_setup, args, kwargs,
|
||||
exp_u_aux):
|
||||
# Test add_speeds
|
||||
self.system.add_auxiliary_speeds(*args, **kwargs)
|
||||
assert self.system.u_aux[:] == exp_u_aux
|
||||
self._empty_system_check(exclude=('u_aux',))
|
||||
# Test setter for u_ind and u_dep
|
||||
self.system.u_aux = exp_u_aux
|
||||
assert self.system.u_aux[:] == exp_u_aux
|
||||
self._empty_system_check(exclude=('u_aux',))
|
||||
|
||||
@pytest.mark.parametrize('args, kwargs', [
|
||||
((ua[2], q[0]), {}),
|
||||
((ua[2], u[1]), {}),
|
||||
((ua[0], ua[2]), {}),
|
||||
((symbols('a'), ua[2]), {}),
|
||||
])
|
||||
def test_auxiliary_invalid(self, _filled_system_setup, args, kwargs):
|
||||
with pytest.raises(ValueError):
|
||||
self.system.add_auxiliary_speeds(*args, **kwargs)
|
||||
self._filled_system_check()
|
||||
|
||||
@pytest.mark.parametrize('prop, add_func, args, kwargs', [
|
||||
('q_ind', 'add_coordinates', (q[0],), {}),
|
||||
('q_dep', 'add_coordinates', (q[3],), {'independent': False}),
|
||||
('u_ind', 'add_speeds', (u[0],), {}),
|
||||
('u_dep', 'add_speeds', (u[3],), {'independent': False}),
|
||||
('u_aux', 'add_auxiliary_speeds', (ua[2],), {}),
|
||||
('kdes', 'add_kdes', (qd[0] - u[0],), {}),
|
||||
('holonomic_constraints', 'add_holonomic_constraints',
|
||||
(q[0] - q[1],), {}),
|
||||
('nonholonomic_constraints', 'add_nonholonomic_constraints',
|
||||
(u[0] - u[1],), {}),
|
||||
('bodies', 'add_bodies', (RigidBody('body'),), {}),
|
||||
('loads', 'add_loads', (Force(Point('P'), ReferenceFrame('N').x),), {}),
|
||||
('actuators', 'add_actuators', (TorqueActuator(
|
||||
symbols('T'), ReferenceFrame('N').x, ReferenceFrame('A')),), {}),
|
||||
])
|
||||
def test_add_after_reset(self, _filled_system_setup, prop, add_func, args,
|
||||
kwargs):
|
||||
setattr(self.system, prop, ())
|
||||
exclude = (prop, 'q', 'u')
|
||||
if prop in ('holonomic_constraints', 'nonholonomic_constraints'):
|
||||
exclude += ('velocity_constraints',)
|
||||
self._filled_system_check(exclude=exclude)
|
||||
assert list(getattr(self.system, prop)[:]) == []
|
||||
getattr(self.system, add_func)(*args, **kwargs)
|
||||
assert list(getattr(self.system, prop)[:]) == list(args)
|
||||
|
||||
@pytest.mark.parametrize('prop, add_func, value, error', [
|
||||
('q_ind', 'add_coordinates', symbols('a'), ValueError),
|
||||
('q_dep', 'add_coordinates', symbols('a'), ValueError),
|
||||
('u_ind', 'add_speeds', symbols('a'), ValueError),
|
||||
('u_dep', 'add_speeds', symbols('a'), ValueError),
|
||||
('u_aux', 'add_auxiliary_speeds', symbols('a'), ValueError),
|
||||
('kdes', 'add_kdes', 7, TypeError),
|
||||
('holonomic_constraints', 'add_holonomic_constraints', 7, TypeError),
|
||||
('nonholonomic_constraints', 'add_nonholonomic_constraints', 7,
|
||||
TypeError),
|
||||
('bodies', 'add_bodies', symbols('a'), TypeError),
|
||||
('loads', 'add_loads', symbols('a'), TypeError),
|
||||
('actuators', 'add_actuators', symbols('a'), TypeError),
|
||||
])
|
||||
def test_type_error(self, _filled_system_setup, prop, add_func, value,
|
||||
error):
|
||||
with pytest.raises(error):
|
||||
getattr(self.system, add_func)(value)
|
||||
with pytest.raises(error):
|
||||
setattr(self.system, prop, value)
|
||||
self._filled_system_check()
|
||||
|
||||
@pytest.mark.parametrize('args, kwargs, exp_kdes', [
|
||||
((), {}, [ui - qdi for ui, qdi in zip(u[:4], qd[:4])]),
|
||||
((u[4] - qd[4], u[5] - qd[5]), {},
|
||||
[ui - qdi for ui, qdi in zip(u[:6], qd[:6])]),
|
||||
])
|
||||
def test_kdes(self, _filled_system_setup, args, kwargs, exp_kdes):
|
||||
# Test add_speeds
|
||||
self.system.add_kdes(*args, **kwargs)
|
||||
self._filled_system_check(exclude=('kdes',))
|
||||
assert self.system.kdes[:] == exp_kdes
|
||||
# Test setter for kdes
|
||||
self.system.kdes = exp_kdes
|
||||
self._filled_system_check(exclude=('kdes',))
|
||||
assert self.system.kdes[:] == exp_kdes
|
||||
|
||||
@pytest.mark.parametrize('args, kwargs', [
|
||||
((u[0] - qd[0], u[4] - qd[4]), {}),
|
||||
((-(u[0] - qd[0]), u[4] - qd[4]), {}),
|
||||
(([u[0] - u[0], u[4] - qd[4]]), {}),
|
||||
])
|
||||
def test_kdes_invalid(self, _filled_system_setup, args, kwargs):
|
||||
with pytest.raises(ValueError):
|
||||
self.system.add_kdes(*args, **kwargs)
|
||||
self._filled_system_check()
|
||||
|
||||
@pytest.mark.parametrize('args, kwargs, exp_con', [
|
||||
((), {}, [q[2] - q[0] + q[1]]),
|
||||
((q[4] - q[5], q[5] + q[3]), {},
|
||||
[q[2] - q[0] + q[1], q[4] - q[5], q[5] + q[3]]),
|
||||
])
|
||||
def test_holonomic_constraints(self, _filled_system_setup, args, kwargs,
|
||||
exp_con):
|
||||
exclude = ('holonomic_constraints', 'velocity_constraints')
|
||||
exp_vel_con = [c.diff(t) for c in exp_con] + self.nhc
|
||||
# Test add_holonomic_constraints
|
||||
self.system.add_holonomic_constraints(*args, **kwargs)
|
||||
self._filled_system_check(exclude=exclude)
|
||||
assert self.system.holonomic_constraints[:] == exp_con
|
||||
assert self.system.velocity_constraints[:] == exp_vel_con
|
||||
# Test setter for holonomic_constraints
|
||||
self.system.holonomic_constraints = exp_con
|
||||
self._filled_system_check(exclude=exclude)
|
||||
assert self.system.holonomic_constraints[:] == exp_con
|
||||
assert self.system.velocity_constraints[:] == exp_vel_con
|
||||
|
||||
@pytest.mark.parametrize('args, kwargs', [
|
||||
((q[2] - q[0] + q[1], q[4] - q[3]), {}),
|
||||
((-(q[2] - q[0] + q[1]), q[4] - q[3]), {}),
|
||||
((q[0] - q[0], q[4] - q[3]), {}),
|
||||
])
|
||||
def test_holonomic_constraints_invalid(self, _filled_system_setup, args,
|
||||
kwargs):
|
||||
with pytest.raises(ValueError):
|
||||
self.system.add_holonomic_constraints(*args, **kwargs)
|
||||
self._filled_system_check()
|
||||
|
||||
@pytest.mark.parametrize('args, kwargs, exp_con', [
|
||||
((), {}, [u[3] - qd[1] + u[2]]),
|
||||
((u[4] - u[5], u[5] + u[3]), {},
|
||||
[u[3] - qd[1] + u[2], u[4] - u[5], u[5] + u[3]]),
|
||||
])
|
||||
def test_nonholonomic_constraints(self, _filled_system_setup, args, kwargs,
|
||||
exp_con):
|
||||
exclude = ('nonholonomic_constraints', 'velocity_constraints')
|
||||
exp_vel_con = self.vc[:len(self.hc)] + exp_con
|
||||
# Test add_nonholonomic_constraints
|
||||
self.system.add_nonholonomic_constraints(*args, **kwargs)
|
||||
self._filled_system_check(exclude=exclude)
|
||||
assert self.system.nonholonomic_constraints[:] == exp_con
|
||||
assert self.system.velocity_constraints[:] == exp_vel_con
|
||||
# Test setter for nonholonomic_constraints
|
||||
self.system.nonholonomic_constraints = exp_con
|
||||
self._filled_system_check(exclude=exclude)
|
||||
assert self.system.nonholonomic_constraints[:] == exp_con
|
||||
assert self.system.velocity_constraints[:] == exp_vel_con
|
||||
|
||||
@pytest.mark.parametrize('args, kwargs', [
|
||||
((u[3] - qd[1] + u[2], u[4] - u[3]), {}),
|
||||
((-(u[3] - qd[1] + u[2]), u[4] - u[3]), {}),
|
||||
((u[0] - u[0], u[4] - u[3]), {}),
|
||||
(([u[0] - u[0], u[4] - u[3]]), {}),
|
||||
])
|
||||
def test_nonholonomic_constraints_invalid(self, _filled_system_setup, args,
|
||||
kwargs):
|
||||
with pytest.raises(ValueError):
|
||||
self.system.add_nonholonomic_constraints(*args, **kwargs)
|
||||
self._filled_system_check()
|
||||
|
||||
@pytest.mark.parametrize('constraints, expected', [
|
||||
([], []),
|
||||
(qd[2] - qd[0] + qd[1], [qd[2] - qd[0] + qd[1]]),
|
||||
([qd[2] + qd[1], u[2] - u[1]], [qd[2] + qd[1], u[2] - u[1]]),
|
||||
])
|
||||
def test_velocity_constraints_overwrite(self, _filled_system_setup,
|
||||
constraints, expected):
|
||||
self.system.velocity_constraints = constraints
|
||||
self._filled_system_check(exclude=('velocity_constraints',))
|
||||
assert self.system.velocity_constraints[:] == expected
|
||||
|
||||
def test_velocity_constraints_back_to_auto(self, _filled_system_setup):
|
||||
self.system.velocity_constraints = qd[3] - qd[2]
|
||||
self._filled_system_check(exclude=('velocity_constraints',))
|
||||
assert self.system.velocity_constraints[:] == [qd[3] - qd[2]]
|
||||
self.system.velocity_constraints = None
|
||||
self._filled_system_check()
|
||||
|
||||
def test_bodies(self, _filled_system_setup):
|
||||
rb1, rb2 = RigidBody('rb1'), RigidBody('rb2')
|
||||
p1, p2 = Particle('p1'), Particle('p2')
|
||||
self.system.add_bodies(rb1, p1)
|
||||
assert self.system.bodies == (*self.bodies, rb1, p1)
|
||||
self.system.add_bodies(p2)
|
||||
assert self.system.bodies == (*self.bodies, rb1, p1, p2)
|
||||
self.system.bodies = []
|
||||
assert self.system.bodies == ()
|
||||
self.system.bodies = p2
|
||||
assert self.system.bodies == (p2,)
|
||||
symb = symbols('symb')
|
||||
pytest.raises(TypeError, lambda: self.system.add_bodies(symb))
|
||||
pytest.raises(ValueError, lambda: self.system.add_bodies(p2))
|
||||
with pytest.raises(TypeError):
|
||||
self.system.bodies = (rb1, rb2, p1, p2, symb)
|
||||
assert self.system.bodies == (p2,)
|
||||
|
||||
def test_add_loads(self):
|
||||
system = System()
|
||||
N, A = ReferenceFrame('N'), ReferenceFrame('A')
|
||||
rb1 = RigidBody('rb1', frame=N)
|
||||
mc1 = Point('mc1')
|
||||
p1 = Particle('p1', mc1)
|
||||
system.add_loads(Torque(rb1, N.x), (mc1, A.x), Force(p1, A.x))
|
||||
assert system.loads == ((N, N.x), (mc1, A.x), (mc1, A.x))
|
||||
system.loads = [(A, A.x)]
|
||||
assert system.loads == ((A, A.x),)
|
||||
pytest.raises(ValueError, lambda: system.add_loads((N, N.x, N.y)))
|
||||
with pytest.raises(TypeError):
|
||||
system.loads = (N, N.x)
|
||||
assert system.loads == ((A, A.x),)
|
||||
|
||||
def test_add_actuators(self):
|
||||
system = System()
|
||||
N, A = ReferenceFrame('N'), ReferenceFrame('A')
|
||||
act1 = TorqueActuator(symbols('T1'), N.x, N)
|
||||
act2 = TorqueActuator(symbols('T2'), N.y, N, A)
|
||||
system.add_actuators(act1)
|
||||
assert system.actuators == (act1,)
|
||||
assert system.loads == ()
|
||||
system.actuators = (act2,)
|
||||
assert system.actuators == (act2,)
|
||||
|
||||
def test_add_joints(self):
|
||||
q1, q2, q3, q4, u1, u2, u3 = dynamicsymbols('q1:5 u1:4')
|
||||
rb1, rb2, rb3, rb4, rb5 = symbols('rb1:6', cls=RigidBody)
|
||||
J1 = PinJoint('J1', rb1, rb2, q1, u1)
|
||||
J2 = PrismaticJoint('J2', rb2, rb3, q2, u2)
|
||||
J3 = PinJoint('J3', rb3, rb4, q3, u3)
|
||||
J_lag = PinJoint('J_lag', rb4, rb5, q4, q4.diff(t))
|
||||
system = System()
|
||||
system.add_joints(J1)
|
||||
assert system.joints == (J1,)
|
||||
assert system.bodies == (rb1, rb2)
|
||||
assert system.q_ind == ImmutableMatrix([q1])
|
||||
assert system.u_ind == ImmutableMatrix([u1])
|
||||
assert system.kdes == ImmutableMatrix([u1 - q1.diff(t)])
|
||||
system.add_bodies(rb4)
|
||||
system.add_coordinates(q3)
|
||||
system.add_kdes(u3 - q3.diff(t))
|
||||
system.add_joints(J3)
|
||||
assert system.joints == (J1, J3)
|
||||
assert system.bodies == (rb1, rb2, rb4, rb3)
|
||||
assert system.q_ind == ImmutableMatrix([q1, q3])
|
||||
assert system.u_ind == ImmutableMatrix([u1, u3])
|
||||
assert system.kdes == ImmutableMatrix(
|
||||
[u1 - q1.diff(t), u3 - q3.diff(t)])
|
||||
system.add_kdes(-(u2 - q2.diff(t)))
|
||||
system.add_joints(J2)
|
||||
assert system.joints == (J1, J3, J2)
|
||||
assert system.bodies == (rb1, rb2, rb4, rb3)
|
||||
assert system.q_ind == ImmutableMatrix([q1, q3, q2])
|
||||
assert system.u_ind == ImmutableMatrix([u1, u3, u2])
|
||||
assert system.kdes == ImmutableMatrix([u1 - q1.diff(t), u3 - q3.diff(t),
|
||||
-(u2 - q2.diff(t))])
|
||||
system.add_joints(J_lag)
|
||||
assert system.joints == (J1, J3, J2, J_lag)
|
||||
assert system.bodies == (rb1, rb2, rb4, rb3, rb5)
|
||||
assert system.q_ind == ImmutableMatrix([q1, q3, q2, q4])
|
||||
assert system.u_ind == ImmutableMatrix([u1, u3, u2, q4.diff(t)])
|
||||
assert system.kdes == ImmutableMatrix([u1 - q1.diff(t), u3 - q3.diff(t),
|
||||
-(u2 - q2.diff(t))])
|
||||
assert system.q_dep[:] == []
|
||||
assert system.u_dep[:] == []
|
||||
pytest.raises(ValueError, lambda: system.add_joints(J2))
|
||||
pytest.raises(TypeError, lambda: system.add_joints(rb1))
|
||||
|
||||
def test_joints_setter(self, _filled_system_setup):
|
||||
self.system.joints = self.joints[1:]
|
||||
assert self.system.joints == self.joints[1:]
|
||||
self._filled_system_check(exclude=('joints',))
|
||||
self.system.q_ind = ()
|
||||
self.system.u_ind = ()
|
||||
self.system.joints = self.joints
|
||||
self._filled_system_check()
|
||||
|
||||
@pytest.mark.parametrize('name, joint_index', [
|
||||
('J1', 0),
|
||||
('J2', 1),
|
||||
('not_existing', None),
|
||||
])
|
||||
def test_get_joint(self, _filled_system_setup, name, joint_index):
|
||||
joint = self.system.get_joint(name)
|
||||
if joint_index is None:
|
||||
assert joint is None
|
||||
else:
|
||||
assert joint == self.joints[joint_index]
|
||||
|
||||
@pytest.mark.parametrize('name, body_index', [
|
||||
('rb1', 0),
|
||||
('rb3', 2),
|
||||
('not_existing', None),
|
||||
])
|
||||
def test_get_body(self, _filled_system_setup, name, body_index):
|
||||
body = self.system.get_body(name)
|
||||
if body_index is None:
|
||||
assert body is None
|
||||
else:
|
||||
assert body == self.bodies[body_index]
|
||||
|
||||
@pytest.mark.parametrize('eom_method', [KanesMethod, LagrangesMethod])
|
||||
def test_form_eoms_calls_subclass(self, _moving_point_mass, eom_method):
|
||||
class MyMethod(eom_method):
|
||||
pass
|
||||
|
||||
self.system.form_eoms(eom_method=MyMethod)
|
||||
assert isinstance(self.system.eom_method, MyMethod)
|
||||
|
||||
@pytest.mark.parametrize('kwargs, expected', [
|
||||
({}, ImmutableMatrix([[-1, 0], [0, symbols('m')]])),
|
||||
({'explicit_kinematics': True}, ImmutableMatrix([[1, 0],
|
||||
[0, symbols('m')]])),
|
||||
])
|
||||
def test_system_kane_form_eoms_kwargs(self, _moving_point_mass, kwargs,
|
||||
expected):
|
||||
self.system.form_eoms(**kwargs)
|
||||
assert self.system.mass_matrix_full == expected
|
||||
|
||||
@pytest.mark.parametrize('kwargs, mm, gm', [
|
||||
({}, ImmutableMatrix([[1, 0], [0, symbols('m')]]),
|
||||
ImmutableMatrix([q[0].diff(t), 0])),
|
||||
])
|
||||
def test_system_lagrange_form_eoms_kwargs(self, _moving_point_mass, kwargs,
|
||||
mm, gm):
|
||||
self.system.form_eoms(eom_method=LagrangesMethod, **kwargs)
|
||||
assert self.system.mass_matrix_full == mm
|
||||
assert self.system.forcing_full == gm
|
||||
|
||||
@pytest.mark.parametrize('eom_method, kwargs, error', [
|
||||
(KanesMethod, {'non_existing_kwarg': 1}, TypeError),
|
||||
(LagrangesMethod, {'non_existing_kwarg': 1}, TypeError),
|
||||
(KanesMethod, {'bodies': []}, ValueError),
|
||||
(KanesMethod, {'kd_eqs': []}, ValueError),
|
||||
(LagrangesMethod, {'bodies': []}, ValueError),
|
||||
(LagrangesMethod, {'Lagrangian': 1}, ValueError),
|
||||
])
|
||||
def test_form_eoms_kwargs_errors(self, _empty_system_setup, eom_method,
|
||||
kwargs, error):
|
||||
self.system.q_ind = q[0]
|
||||
p = Particle('p', mass=symbols('m'))
|
||||
self.system.add_bodies(p)
|
||||
p.masscenter.set_pos(self.system.fixed_point, q[0] * self.system.x)
|
||||
with pytest.raises(error):
|
||||
self.system.form_eoms(eom_method=eom_method, **kwargs)
|
||||
|
||||
|
||||
class TestValidateSystem(TestSystemBase):
|
||||
@pytest.mark.parametrize('valid_method, invalid_method, with_speeds', [
|
||||
(KanesMethod, LagrangesMethod, True),
|
||||
(LagrangesMethod, KanesMethod, False)
|
||||
])
|
||||
def test_only_valid(self, valid_method, invalid_method, with_speeds):
|
||||
self._create_filled_system(with_speeds=with_speeds)
|
||||
self.system.validate_system(valid_method)
|
||||
# Test Lagrange should fail due to the usage of generalized speeds
|
||||
with pytest.raises(ValueError):
|
||||
self.system.validate_system(invalid_method)
|
||||
|
||||
@pytest.mark.parametrize('method, with_speeds', [
|
||||
(KanesMethod, True), (LagrangesMethod, False)])
|
||||
def test_missing_joint_coordinate(self, method, with_speeds):
|
||||
self._create_filled_system(with_speeds=with_speeds)
|
||||
self.system.q_ind = self.q_ind[1:]
|
||||
self.system.u_ind = self.u_ind[:-1]
|
||||
self.system.kdes = self.kdes[:-1]
|
||||
pytest.raises(ValueError, lambda: self.system.validate_system(method))
|
||||
|
||||
def test_missing_joint_speed(self, _filled_system_setup):
|
||||
self.system.q_ind = self.q_ind[:-1]
|
||||
self.system.u_ind = self.u_ind[1:]
|
||||
self.system.kdes = self.kdes[:-1]
|
||||
pytest.raises(ValueError, lambda: self.system.validate_system())
|
||||
|
||||
def test_missing_joint_kdes(self, _filled_system_setup):
|
||||
self.system.kdes = self.kdes[1:]
|
||||
pytest.raises(ValueError, lambda: self.system.validate_system())
|
||||
|
||||
def test_negative_joint_kdes(self, _filled_system_setup):
|
||||
self.system.kdes = [-self.kdes[0]] + self.kdes[1:]
|
||||
self.system.validate_system()
|
||||
|
||||
@pytest.mark.parametrize('method, with_speeds', [
|
||||
(KanesMethod, True), (LagrangesMethod, False)])
|
||||
def test_missing_holonomic_constraint(self, method, with_speeds):
|
||||
self._create_filled_system(with_speeds=with_speeds)
|
||||
self.system.holonomic_constraints = []
|
||||
self.system.nonholonomic_constraints = self.nhc + [
|
||||
self.u_ind[1] - self.u_dep[0] + self.u_ind[0]]
|
||||
pytest.raises(ValueError, lambda: self.system.validate_system(method))
|
||||
self.system.q_dep = []
|
||||
self.system.q_ind = self.q_ind + self.q_dep
|
||||
self.system.validate_system(method)
|
||||
|
||||
def test_missing_nonholonomic_constraint(self, _filled_system_setup):
|
||||
self.system.nonholonomic_constraints = []
|
||||
pytest.raises(ValueError, lambda: self.system.validate_system())
|
||||
self.system.u_dep = self.u_dep[1]
|
||||
self.system.u_ind = self.u_ind + [self.u_dep[0]]
|
||||
self.system.validate_system()
|
||||
|
||||
def test_number_of_coordinates_speeds(self, _filled_system_setup):
|
||||
# Test more speeds than coordinates
|
||||
self.system.u_ind = self.u_ind + [u[5]]
|
||||
self.system.kdes = self.kdes + [u[5] - qd[5]]
|
||||
self.system.validate_system()
|
||||
# Test more coordinates than speeds
|
||||
self.system.q_ind = self.q_ind
|
||||
self.system.u_ind = self.u_ind[:-1]
|
||||
self.system.kdes = self.kdes[:-1]
|
||||
pytest.raises(ValueError, lambda: self.system.validate_system())
|
||||
|
||||
def test_number_of_kdes(self, _filled_system_setup):
|
||||
# Test wrong number of kdes
|
||||
self.system.kdes = self.kdes[:-1]
|
||||
pytest.raises(ValueError, lambda: self.system.validate_system())
|
||||
self.system.kdes = self.kdes + [u[2] + u[1] - qd[2]]
|
||||
pytest.raises(ValueError, lambda: self.system.validate_system())
|
||||
|
||||
def test_duplicates(self, _filled_system_setup):
|
||||
# This is basically a redundant feature, which should never fail
|
||||
self.system.validate_system(check_duplicates=True)
|
||||
|
||||
def test_speeds_in_lagrange(self, _filled_system_setup_no_speeds):
|
||||
self.system.u_ind = u[:len(self.u_ind)]
|
||||
with pytest.raises(ValueError):
|
||||
self.system.validate_system(LagrangesMethod)
|
||||
self.system.u_ind = []
|
||||
self.system.validate_system(LagrangesMethod)
|
||||
self.system.u_aux = ua
|
||||
with pytest.raises(ValueError):
|
||||
self.system.validate_system(LagrangesMethod)
|
||||
self.system.u_aux = []
|
||||
self.system.validate_system(LagrangesMethod)
|
||||
self.system.add_joints(
|
||||
PinJoint('Ju', RigidBody('rbu1'), RigidBody('rbu2')))
|
||||
self.system.u_ind = []
|
||||
with pytest.raises(ValueError):
|
||||
self.system.validate_system(LagrangesMethod)
|
||||
|
||||
|
||||
class TestSystemExamples:
|
||||
def test_cart_pendulum_kanes(self):
|
||||
# This example is the same as in the top documentation of System
|
||||
# Added a spring to the cart
|
||||
g, l, mc, mp, k = symbols('g l mc mp k')
|
||||
F, qp, qc, up, uc = dynamicsymbols('F qp qc up uc')
|
||||
rail = RigidBody('rail')
|
||||
cart = RigidBody('cart', mass=mc)
|
||||
bob = Particle('bob', mass=mp)
|
||||
bob_frame = ReferenceFrame('bob_frame')
|
||||
system = System.from_newtonian(rail)
|
||||
assert system.bodies == (rail,)
|
||||
assert system.frame == rail.frame
|
||||
assert system.fixed_point == rail.masscenter
|
||||
slider = PrismaticJoint('slider', rail, cart, qc, uc, joint_axis=rail.x)
|
||||
pin = PinJoint('pin', cart, bob, qp, up, joint_axis=cart.z,
|
||||
child_interframe=bob_frame, child_point=l * bob_frame.y)
|
||||
system.add_joints(slider, pin)
|
||||
assert system.joints == (slider, pin)
|
||||
assert system.get_joint('slider') == slider
|
||||
assert system.get_body('bob') == bob
|
||||
system.apply_uniform_gravity(-g * system.y)
|
||||
system.add_loads((cart.masscenter, F * rail.x))
|
||||
system.add_actuators(TorqueActuator(k * qp, cart.z, bob_frame, cart))
|
||||
system.validate_system()
|
||||
system.form_eoms()
|
||||
assert isinstance(system.eom_method, KanesMethod)
|
||||
assert (simplify(system.mass_matrix - ImmutableMatrix(
|
||||
[[mp + mc, mp * l * cos(qp)], [mp * l * cos(qp), mp * l ** 2]]))
|
||||
== zeros(2, 2))
|
||||
assert (simplify(system.forcing - ImmutableMatrix([
|
||||
[mp * l * up ** 2 * sin(qp) + F],
|
||||
[-mp * g * l * sin(qp) + k * qp]])) == zeros(2, 1))
|
||||
|
||||
system.add_holonomic_constraints(
|
||||
sympify(bob.masscenter.pos_from(rail.masscenter).dot(system.x)))
|
||||
assert system.eom_method is None
|
||||
system.q_ind, system.q_dep = qp, qc
|
||||
system.u_ind, system.u_dep = up, uc
|
||||
system.validate_system()
|
||||
|
||||
# Computed solution based on manually solving the constraints
|
||||
subs = {qc: -l * sin(qp),
|
||||
uc: -l * cos(qp) * up,
|
||||
uc.diff(t): l * (up ** 2 * sin(qp) - up.diff(t) * cos(qp))}
|
||||
upd_expected = (
|
||||
(-g * mp * sin(qp) + k * qp / l + l * mc * sin(2 * qp) * up ** 2 / 2
|
||||
- l * mp * sin(2 * qp) * up ** 2 / 2 - F * cos(qp)) /
|
||||
(l * (mc * cos(qp) ** 2 + mp * sin(qp) ** 2)))
|
||||
upd_sol = tuple(solve(system.form_eoms().xreplace(subs),
|
||||
up.diff(t)).values())[0]
|
||||
assert simplify(upd_sol - upd_expected) == 0
|
||||
assert isinstance(system.eom_method, KanesMethod)
|
||||
|
||||
# Test other output
|
||||
Mk = -ImmutableMatrix([[0, 1], [1, 0]])
|
||||
gk = -ImmutableMatrix([uc, up])
|
||||
Md = ImmutableMatrix([[-l ** 2 * mp * cos(qp) ** 2 + l ** 2 * mp,
|
||||
l * mp * cos(qp) - l * (mc + mp) * cos(qp)],
|
||||
[l * cos(qp), 1]])
|
||||
gd = ImmutableMatrix(
|
||||
[[-g * l * mp * sin(qp) + k * qp - l ** 2 * mp * up ** 2 * sin(qp) *
|
||||
cos(qp) - l * F * cos(qp)], [l * up ** 2 * sin(qp)]])
|
||||
Mm = (Mk.row_join(zeros(2, 2))).col_join(zeros(2, 2).row_join(Md))
|
||||
gm = gk.col_join(gd)
|
||||
assert simplify(system.mass_matrix - Md) == zeros(2, 2)
|
||||
assert simplify(system.forcing - gd) == zeros(2, 1)
|
||||
assert simplify(system.mass_matrix_full - Mm) == zeros(4, 4)
|
||||
assert simplify(system.forcing_full - gm) == zeros(4, 1)
|
||||
|
||||
def test_cart_pendulum_lagrange(self):
|
||||
# Lagrange version of test_cart_pendulus_kanes
|
||||
# Added a spring to the cart
|
||||
g, l, mc, mp, k = symbols('g l mc mp k')
|
||||
F, qp, qc = dynamicsymbols('F qp qc')
|
||||
qpd, qcd = dynamicsymbols('qp qc', 1)
|
||||
rail = RigidBody('rail')
|
||||
cart = RigidBody('cart', mass=mc)
|
||||
bob = Particle('bob', mass=mp)
|
||||
bob_frame = ReferenceFrame('bob_frame')
|
||||
system = System.from_newtonian(rail)
|
||||
assert system.bodies == (rail,)
|
||||
assert system.frame == rail.frame
|
||||
assert system.fixed_point == rail.masscenter
|
||||
slider = PrismaticJoint('slider', rail, cart, qc, qcd,
|
||||
joint_axis=rail.x)
|
||||
pin = PinJoint('pin', cart, bob, qp, qpd, joint_axis=cart.z,
|
||||
child_interframe=bob_frame, child_point=l * bob_frame.y)
|
||||
system.add_joints(slider, pin)
|
||||
assert system.joints == (slider, pin)
|
||||
assert system.get_joint('slider') == slider
|
||||
assert system.get_body('bob') == bob
|
||||
for body in system.bodies:
|
||||
body.potential_energy = body.mass * g * body.masscenter.pos_from(
|
||||
system.fixed_point).dot(system.y)
|
||||
system.add_loads((cart.masscenter, F * rail.x))
|
||||
system.add_actuators(TorqueActuator(k * qp, cart.z, bob_frame, cart))
|
||||
system.validate_system(LagrangesMethod)
|
||||
system.form_eoms(LagrangesMethod)
|
||||
assert (simplify(system.mass_matrix - ImmutableMatrix(
|
||||
[[mp + mc, mp * l * cos(qp)], [mp * l * cos(qp), mp * l ** 2]]))
|
||||
== zeros(2, 2))
|
||||
assert (simplify(system.forcing - ImmutableMatrix([
|
||||
[mp * l * qpd ** 2 * sin(qp) + F], [-mp * g * l * sin(qp) + k * qp]]
|
||||
)) == zeros(2, 1))
|
||||
|
||||
system.add_holonomic_constraints(
|
||||
sympify(bob.masscenter.pos_from(rail.masscenter).dot(system.x)))
|
||||
assert system.eom_method is None
|
||||
system.q_ind, system.q_dep = qp, qc
|
||||
|
||||
# Computed solution based on manually solving the constraints
|
||||
subs = {qc: -l * sin(qp),
|
||||
qcd: -l * cos(qp) * qpd,
|
||||
qcd.diff(t): l * (qpd ** 2 * sin(qp) - qpd.diff(t) * cos(qp))}
|
||||
qpdd_expected = (
|
||||
(-g * mp * sin(qp) + k * qp / l + l * mc * sin(2 * qp) * qpd ** 2 /
|
||||
2 - l * mp * sin(2 * qp) * qpd ** 2 / 2 - F * cos(qp)) /
|
||||
(l * (mc * cos(qp) ** 2 + mp * sin(qp) ** 2)))
|
||||
eoms = system.form_eoms(LagrangesMethod)
|
||||
lam1 = system.eom_method.lam_vec[0]
|
||||
lam1_sol = system.eom_method.solve_multipliers()[lam1]
|
||||
qpdd_sol = solve(eoms[0].xreplace({lam1: lam1_sol}).xreplace(subs),
|
||||
qpd.diff(t))[0]
|
||||
assert simplify(qpdd_sol - qpdd_expected) == 0
|
||||
assert isinstance(system.eom_method, LagrangesMethod)
|
||||
|
||||
# Test other output
|
||||
Md = ImmutableMatrix([[l ** 2 * mp, l * mp * cos(qp), -l * cos(qp)],
|
||||
[l * mp * cos(qp), mc + mp, -1]])
|
||||
gd = ImmutableMatrix(
|
||||
[[-g * l * mp * sin(qp) + k * qp],
|
||||
[l * mp * sin(qp) * qpd ** 2 + F]])
|
||||
Mm = (eye(2).row_join(zeros(2, 3))).col_join(zeros(3, 2).row_join(
|
||||
Md.col_join(ImmutableMatrix([l * cos(qp), 1, 0]).T)))
|
||||
gm = ImmutableMatrix([qpd, qcd] + gd[:] + [l * sin(qp) * qpd ** 2])
|
||||
assert simplify(system.mass_matrix - Md) == zeros(2, 3)
|
||||
assert simplify(system.forcing - gd) == zeros(2, 1)
|
||||
assert simplify(system.mass_matrix_full - Mm) == zeros(5, 5)
|
||||
assert simplify(system.forcing_full - gm) == zeros(5, 1)
|
||||
|
||||
def test_box_on_ground(self):
|
||||
# Particle sliding on ground with friction. The applied force is assumed
|
||||
# to be positive and to be higher than the friction force.
|
||||
g, m, mu = symbols('g m mu')
|
||||
q, u, ua = dynamicsymbols('q u ua')
|
||||
N, F = dynamicsymbols('N F', positive=True)
|
||||
P = Particle("P", mass=m)
|
||||
system = System()
|
||||
system.add_bodies(P)
|
||||
P.masscenter.set_pos(system.fixed_point, q * system.x)
|
||||
P.masscenter.set_vel(system.frame, u * system.x + ua * system.y)
|
||||
system.q_ind, system.u_ind, system.u_aux = [q], [u], [ua]
|
||||
system.kdes = [q.diff(t) - u]
|
||||
system.apply_uniform_gravity(-g * system.y)
|
||||
system.add_loads(
|
||||
Force(P, N * system.y),
|
||||
Force(P, F * system.x - mu * N * system.x))
|
||||
system.validate_system()
|
||||
system.form_eoms()
|
||||
|
||||
# Test other output
|
||||
Mk = ImmutableMatrix([1])
|
||||
gk = ImmutableMatrix([u])
|
||||
Md = ImmutableMatrix([m])
|
||||
gd = ImmutableMatrix([F - mu * N])
|
||||
Mm = (Mk.row_join(zeros(1, 1))).col_join(zeros(1, 1).row_join(Md))
|
||||
gm = gk.col_join(gd)
|
||||
aux_eqs = ImmutableMatrix([N - m * g])
|
||||
assert simplify(system.mass_matrix - Md) == zeros(1, 1)
|
||||
assert simplify(system.forcing - gd) == zeros(1, 1)
|
||||
assert simplify(system.mass_matrix_full - Mm) == zeros(2, 2)
|
||||
assert simplify(system.forcing_full - gm) == zeros(2, 1)
|
||||
assert simplify(system.eom_method.auxiliary_eqs - aux_eqs
|
||||
) == zeros(1, 1)
|
||||
+363
@@ -0,0 +1,363 @@
|
||||
"""Tests for the ``sympy.physics.mechanics.wrapping_geometry.py`` module."""
|
||||
|
||||
import pytest
|
||||
|
||||
from sympy import (
|
||||
Integer,
|
||||
Rational,
|
||||
S,
|
||||
Symbol,
|
||||
acos,
|
||||
cos,
|
||||
pi,
|
||||
sin,
|
||||
sqrt,
|
||||
)
|
||||
from sympy.core.relational import Eq
|
||||
from sympy.physics.mechanics import (
|
||||
Point,
|
||||
ReferenceFrame,
|
||||
WrappingCylinder,
|
||||
WrappingSphere,
|
||||
dynamicsymbols,
|
||||
)
|
||||
from sympy.simplify.simplify import simplify
|
||||
|
||||
|
||||
r = Symbol('r', positive=True)
|
||||
x = Symbol('x')
|
||||
q = dynamicsymbols('q')
|
||||
N = ReferenceFrame('N')
|
||||
|
||||
|
||||
class TestWrappingSphere:
|
||||
|
||||
@staticmethod
|
||||
def test_valid_constructor():
|
||||
r = Symbol('r', positive=True)
|
||||
pO = Point('pO')
|
||||
sphere = WrappingSphere(r, pO)
|
||||
assert isinstance(sphere, WrappingSphere)
|
||||
assert hasattr(sphere, 'radius')
|
||||
assert sphere.radius == r
|
||||
assert hasattr(sphere, 'point')
|
||||
assert sphere.point == pO
|
||||
|
||||
@staticmethod
|
||||
@pytest.mark.parametrize('position', [S.Zero, Integer(2)*r*N.x])
|
||||
def test_geodesic_length_point_not_on_surface_invalid(position):
|
||||
r = Symbol('r', positive=True)
|
||||
pO = Point('pO')
|
||||
sphere = WrappingSphere(r, pO)
|
||||
|
||||
p1 = Point('p1')
|
||||
p1.set_pos(pO, position)
|
||||
p2 = Point('p2')
|
||||
p2.set_pos(pO, position)
|
||||
|
||||
error_msg = r'point .* does not lie on the surface of'
|
||||
with pytest.raises(ValueError, match=error_msg):
|
||||
sphere.geodesic_length(p1, p2)
|
||||
|
||||
@staticmethod
|
||||
@pytest.mark.parametrize(
|
||||
'position_1, position_2, expected',
|
||||
[
|
||||
(r*N.x, r*N.x, S.Zero),
|
||||
(r*N.x, r*N.y, S.Half*pi*r),
|
||||
(r*N.x, r*-N.x, pi*r),
|
||||
(r*-N.x, r*N.x, pi*r),
|
||||
(r*N.x, r*sqrt(2)*S.Half*(N.x + N.y), Rational(1, 4)*pi*r),
|
||||
(
|
||||
r*sqrt(2)*S.Half*(N.x + N.y),
|
||||
r*sqrt(3)*Rational(1, 3)*(N.x + N.y + N.z),
|
||||
r*acos(sqrt(6)*Rational(1, 3)),
|
||||
),
|
||||
]
|
||||
)
|
||||
def test_geodesic_length(position_1, position_2, expected):
|
||||
r = Symbol('r', positive=True)
|
||||
pO = Point('pO')
|
||||
sphere = WrappingSphere(r, pO)
|
||||
|
||||
p1 = Point('p1')
|
||||
p1.set_pos(pO, position_1)
|
||||
p2 = Point('p2')
|
||||
p2.set_pos(pO, position_2)
|
||||
|
||||
assert simplify(Eq(sphere.geodesic_length(p1, p2), expected))
|
||||
|
||||
@staticmethod
|
||||
@pytest.mark.parametrize(
|
||||
'position_1, position_2, vector_1, vector_2',
|
||||
[
|
||||
(r * N.x, r * N.y, N.y, N.x),
|
||||
(r * N.x, -r * N.y, -N.y, N.x),
|
||||
(
|
||||
r * N.y,
|
||||
sqrt(2)/2 * r * N.x - sqrt(2)/2 * r * N.y,
|
||||
N.x,
|
||||
sqrt(2)/2 * N.x + sqrt(2)/2 * N.y,
|
||||
),
|
||||
(
|
||||
r * N.x,
|
||||
r / 2 * N.x + sqrt(3)/2 * r * N.y,
|
||||
N.y,
|
||||
sqrt(3)/2 * N.x - 1/2 * N.y,
|
||||
),
|
||||
(
|
||||
r * N.x,
|
||||
sqrt(2)/2 * r * N.x + sqrt(2)/2 * r * N.y,
|
||||
N.y,
|
||||
sqrt(2)/2 * N.x - sqrt(2)/2 * N.y,
|
||||
),
|
||||
]
|
||||
)
|
||||
def test_geodesic_end_vectors(position_1, position_2, vector_1, vector_2):
|
||||
r = Symbol('r', positive=True)
|
||||
pO = Point('pO')
|
||||
sphere = WrappingSphere(r, pO)
|
||||
|
||||
p1 = Point('p1')
|
||||
p1.set_pos(pO, position_1)
|
||||
p2 = Point('p2')
|
||||
p2.set_pos(pO, position_2)
|
||||
|
||||
expected = (vector_1, vector_2)
|
||||
|
||||
assert sphere.geodesic_end_vectors(p1, p2) == expected
|
||||
|
||||
@staticmethod
|
||||
@pytest.mark.parametrize(
|
||||
'position',
|
||||
[r * N.x, r * cos(q) * N.x + r * sin(q) * N.y]
|
||||
)
|
||||
def test_geodesic_end_vectors_invalid_coincident(position):
|
||||
r = Symbol('r', positive=True)
|
||||
pO = Point('pO')
|
||||
sphere = WrappingSphere(r, pO)
|
||||
|
||||
p1 = Point('p1')
|
||||
p1.set_pos(pO, position)
|
||||
p2 = Point('p2')
|
||||
p2.set_pos(pO, position)
|
||||
|
||||
with pytest.raises(ValueError):
|
||||
_ = sphere.geodesic_end_vectors(p1, p2)
|
||||
|
||||
@staticmethod
|
||||
@pytest.mark.parametrize(
|
||||
'position_1, position_2',
|
||||
[
|
||||
(r * N.x, -r * N.x),
|
||||
(-r * N.y, r * N.y),
|
||||
(
|
||||
r * cos(q) * N.x + r * sin(q) * N.y,
|
||||
-r * cos(q) * N.x - r * sin(q) * N.y,
|
||||
)
|
||||
]
|
||||
)
|
||||
def test_geodesic_end_vectors_invalid_diametrically_opposite(
|
||||
position_1,
|
||||
position_2,
|
||||
):
|
||||
r = Symbol('r', positive=True)
|
||||
pO = Point('pO')
|
||||
sphere = WrappingSphere(r, pO)
|
||||
|
||||
p1 = Point('p1')
|
||||
p1.set_pos(pO, position_1)
|
||||
p2 = Point('p2')
|
||||
p2.set_pos(pO, position_2)
|
||||
|
||||
with pytest.raises(ValueError):
|
||||
_ = sphere.geodesic_end_vectors(p1, p2)
|
||||
|
||||
|
||||
class TestWrappingCylinder:
|
||||
|
||||
@staticmethod
|
||||
def test_valid_constructor():
|
||||
N = ReferenceFrame('N')
|
||||
r = Symbol('r', positive=True)
|
||||
pO = Point('pO')
|
||||
cylinder = WrappingCylinder(r, pO, N.x)
|
||||
assert isinstance(cylinder, WrappingCylinder)
|
||||
assert hasattr(cylinder, 'radius')
|
||||
assert cylinder.radius == r
|
||||
assert hasattr(cylinder, 'point')
|
||||
assert cylinder.point == pO
|
||||
assert hasattr(cylinder, 'axis')
|
||||
assert cylinder.axis == N.x
|
||||
|
||||
@staticmethod
|
||||
@pytest.mark.parametrize(
|
||||
'position, expected',
|
||||
[
|
||||
(S.Zero, False),
|
||||
(r*N.y, True),
|
||||
(r*N.z, True),
|
||||
(r*(N.y + N.z).normalize(), True),
|
||||
(Integer(2)*r*N.y, False),
|
||||
(r*(N.x + N.y), True),
|
||||
(r*(Integer(2)*N.x + N.y), True),
|
||||
(Integer(2)*N.x + r*(Integer(2)*N.y + N.z).normalize(), True),
|
||||
(r*(cos(q)*N.y + sin(q)*N.z), True)
|
||||
]
|
||||
)
|
||||
def test_point_is_on_surface(position, expected):
|
||||
r = Symbol('r', positive=True)
|
||||
pO = Point('pO')
|
||||
cylinder = WrappingCylinder(r, pO, N.x)
|
||||
|
||||
p1 = Point('p1')
|
||||
p1.set_pos(pO, position)
|
||||
|
||||
assert cylinder.point_on_surface(p1) is expected
|
||||
|
||||
@staticmethod
|
||||
@pytest.mark.parametrize('position', [S.Zero, Integer(2)*r*N.y])
|
||||
def test_geodesic_length_point_not_on_surface_invalid(position):
|
||||
r = Symbol('r', positive=True)
|
||||
pO = Point('pO')
|
||||
cylinder = WrappingCylinder(r, pO, N.x)
|
||||
|
||||
p1 = Point('p1')
|
||||
p1.set_pos(pO, position)
|
||||
p2 = Point('p2')
|
||||
p2.set_pos(pO, position)
|
||||
|
||||
error_msg = r'point .* does not lie on the surface of'
|
||||
with pytest.raises(ValueError, match=error_msg):
|
||||
cylinder.geodesic_length(p1, p2)
|
||||
|
||||
@staticmethod
|
||||
@pytest.mark.parametrize(
|
||||
'axis, position_1, position_2, expected',
|
||||
[
|
||||
(N.x, r*N.y, r*N.y, S.Zero),
|
||||
(N.x, r*N.y, N.x + r*N.y, S.One),
|
||||
(N.x, r*N.y, -x*N.x + r*N.y, sqrt(x**2)),
|
||||
(-N.x, r*N.y, x*N.x + r*N.y, sqrt(x**2)),
|
||||
(N.x, r*N.y, r*N.z, S.Half*pi*sqrt(r**2)),
|
||||
(-N.x, r*N.y, r*N.z, Integer(3)*S.Half*pi*sqrt(r**2)),
|
||||
(N.x, r*N.z, r*N.y, Integer(3)*S.Half*pi*sqrt(r**2)),
|
||||
(-N.x, r*N.z, r*N.y, S.Half*pi*sqrt(r**2)),
|
||||
(N.x, r*N.y, r*(cos(q)*N.y + sin(q)*N.z), sqrt(r**2*q**2)),
|
||||
(
|
||||
-N.x, r*N.y,
|
||||
r*(cos(q)*N.y + sin(q)*N.z),
|
||||
sqrt(r**2*(Integer(2)*pi - q)**2),
|
||||
),
|
||||
]
|
||||
)
|
||||
def test_geodesic_length(axis, position_1, position_2, expected):
|
||||
r = Symbol('r', positive=True)
|
||||
pO = Point('pO')
|
||||
cylinder = WrappingCylinder(r, pO, axis)
|
||||
|
||||
p1 = Point('p1')
|
||||
p1.set_pos(pO, position_1)
|
||||
p2 = Point('p2')
|
||||
p2.set_pos(pO, position_2)
|
||||
|
||||
assert simplify(Eq(cylinder.geodesic_length(p1, p2), expected))
|
||||
|
||||
@staticmethod
|
||||
@pytest.mark.parametrize(
|
||||
'axis, position_1, position_2, vector_1, vector_2',
|
||||
[
|
||||
(N.z, r * N.x, r * N.y, N.y, N.x),
|
||||
(N.z, r * N.x, -r * N.x, N.y, N.y),
|
||||
(N.z, -r * N.x, r * N.x, -N.y, -N.y),
|
||||
(-N.z, r * N.x, -r * N.x, -N.y, -N.y),
|
||||
(-N.z, -r * N.x, r * N.x, N.y, N.y),
|
||||
(N.z, r * N.x, -r * N.y, N.y, -N.x),
|
||||
(
|
||||
N.z,
|
||||
r * N.y,
|
||||
sqrt(2)/2 * r * N.x - sqrt(2)/2 * r * N.y,
|
||||
- N.x,
|
||||
- sqrt(2)/2 * N.x - sqrt(2)/2 * N.y,
|
||||
),
|
||||
(
|
||||
N.z,
|
||||
r * N.x,
|
||||
r / 2 * N.x + sqrt(3)/2 * r * N.y,
|
||||
N.y,
|
||||
sqrt(3)/2 * N.x - 1/2 * N.y,
|
||||
),
|
||||
(
|
||||
N.z,
|
||||
r * N.x,
|
||||
sqrt(2)/2 * r * N.x + sqrt(2)/2 * r * N.y,
|
||||
N.y,
|
||||
sqrt(2)/2 * N.x - sqrt(2)/2 * N.y,
|
||||
),
|
||||
(
|
||||
N.z,
|
||||
r * N.x,
|
||||
r * N.x + N.z,
|
||||
N.z,
|
||||
-N.z,
|
||||
),
|
||||
(
|
||||
N.z,
|
||||
r * N.x,
|
||||
r * N.y + pi/2 * r * N.z,
|
||||
sqrt(2)/2 * N.y + sqrt(2)/2 * N.z,
|
||||
sqrt(2)/2 * N.x - sqrt(2)/2 * N.z,
|
||||
),
|
||||
(
|
||||
N.z,
|
||||
r * N.x,
|
||||
r * cos(q) * N.x + r * sin(q) * N.y,
|
||||
N.y,
|
||||
sin(q) * N.x - cos(q) * N.y,
|
||||
),
|
||||
]
|
||||
)
|
||||
def test_geodesic_end_vectors(
|
||||
axis,
|
||||
position_1,
|
||||
position_2,
|
||||
vector_1,
|
||||
vector_2,
|
||||
):
|
||||
r = Symbol('r', positive=True)
|
||||
pO = Point('pO')
|
||||
cylinder = WrappingCylinder(r, pO, axis)
|
||||
|
||||
p1 = Point('p1')
|
||||
p1.set_pos(pO, position_1)
|
||||
p2 = Point('p2')
|
||||
p2.set_pos(pO, position_2)
|
||||
|
||||
expected = (vector_1, vector_2)
|
||||
end_vectors = tuple(
|
||||
end_vector.simplify()
|
||||
for end_vector in cylinder.geodesic_end_vectors(p1, p2)
|
||||
)
|
||||
|
||||
assert end_vectors == expected
|
||||
|
||||
@staticmethod
|
||||
@pytest.mark.parametrize(
|
||||
'axis, position',
|
||||
[
|
||||
(N.z, r * N.x),
|
||||
(N.z, r * cos(q) * N.x + r * sin(q) * N.y + N.z),
|
||||
]
|
||||
)
|
||||
def test_geodesic_end_vectors_invalid_coincident(axis, position):
|
||||
r = Symbol('r', positive=True)
|
||||
pO = Point('pO')
|
||||
cylinder = WrappingCylinder(r, pO, axis)
|
||||
|
||||
p1 = Point('p1')
|
||||
p1.set_pos(pO, position)
|
||||
p2 = Point('p2')
|
||||
p2.set_pos(pO, position)
|
||||
|
||||
with pytest.raises(ValueError):
|
||||
_ = cylinder.geodesic_end_vectors(p1, p2)
|
||||
+641
@@ -0,0 +1,641 @@
|
||||
"""Geometry objects for use by wrapping pathways."""
|
||||
|
||||
from abc import ABC, abstractmethod
|
||||
|
||||
from sympy import Integer, acos, pi, sqrt, sympify, tan
|
||||
from sympy.core.relational import Eq
|
||||
from sympy.functions.elementary.trigonometric import atan2
|
||||
from sympy.polys.polytools import cancel
|
||||
from sympy.physics.vector import Vector, dot
|
||||
from sympy.simplify.simplify import trigsimp
|
||||
|
||||
|
||||
__all__ = [
|
||||
'WrappingGeometryBase',
|
||||
'WrappingCylinder',
|
||||
'WrappingSphere',
|
||||
]
|
||||
|
||||
|
||||
class WrappingGeometryBase(ABC):
|
||||
"""Abstract base class for all geometry classes to inherit from.
|
||||
|
||||
Notes
|
||||
=====
|
||||
|
||||
Instances of this class cannot be directly instantiated by users. However,
|
||||
it can be used to created custom geometry types through subclassing.
|
||||
|
||||
"""
|
||||
|
||||
@property
|
||||
@abstractmethod
|
||||
def point(cls):
|
||||
"""The point with which the geometry is associated."""
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def point_on_surface(self, point):
|
||||
"""Returns ``True`` if a point is on the geometry's surface.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
point : Point
|
||||
The point for which it's to be ascertained if it's on the
|
||||
geometry's surface or not.
|
||||
|
||||
"""
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def geodesic_length(self, point_1, point_2):
|
||||
"""Returns the shortest distance between two points on a geometry's
|
||||
surface.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
point_1 : Point
|
||||
The point from which the geodesic length should be calculated.
|
||||
point_2 : Point
|
||||
The point to which the geodesic length should be calculated.
|
||||
|
||||
"""
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def geodesic_end_vectors(self, point_1, point_2):
|
||||
"""The vectors parallel to the geodesic at the two end points.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
point_1 : Point
|
||||
The point from which the geodesic originates.
|
||||
point_2 : Point
|
||||
The point at which the geodesic terminates.
|
||||
|
||||
"""
|
||||
pass
|
||||
|
||||
def __repr__(self):
|
||||
"""Default representation of a geometry object."""
|
||||
return f'{self.__class__.__name__}()'
|
||||
|
||||
|
||||
class WrappingSphere(WrappingGeometryBase):
|
||||
"""A solid spherical object.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
A wrapping geometry that allows for circular arcs to be defined between
|
||||
pairs of points. These paths are always geodetic (the shortest possible).
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
To create a ``WrappingSphere`` instance, a ``Symbol`` denoting its radius
|
||||
and ``Point`` at which its center will be located are needed:
|
||||
|
||||
>>> from sympy import symbols
|
||||
>>> from sympy.physics.mechanics import Point, WrappingSphere
|
||||
>>> r = symbols('r')
|
||||
>>> pO = Point('pO')
|
||||
|
||||
A sphere with radius ``r`` centered on ``pO`` can be instantiated with:
|
||||
|
||||
>>> WrappingSphere(r, pO)
|
||||
WrappingSphere(radius=r, point=pO)
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
radius : Symbol
|
||||
Radius of the sphere. This symbol must represent a value that is
|
||||
positive and constant, i.e. it cannot be a dynamic symbol, nor can it
|
||||
be an expression.
|
||||
point : Point
|
||||
A point at which the sphere is centered.
|
||||
|
||||
See Also
|
||||
========
|
||||
|
||||
WrappingCylinder: Cylindrical geometry where the wrapping direction can be
|
||||
defined.
|
||||
|
||||
"""
|
||||
|
||||
def __init__(self, radius, point):
|
||||
"""Initializer for ``WrappingSphere``.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
radius : Symbol
|
||||
The radius of the sphere.
|
||||
point : Point
|
||||
A point on which the sphere is centered.
|
||||
|
||||
"""
|
||||
self.radius = radius
|
||||
self.point = point
|
||||
|
||||
@property
|
||||
def radius(self):
|
||||
"""Radius of the sphere."""
|
||||
return self._radius
|
||||
|
||||
@radius.setter
|
||||
def radius(self, radius):
|
||||
self._radius = radius
|
||||
|
||||
@property
|
||||
def point(self):
|
||||
"""A point on which the sphere is centered."""
|
||||
return self._point
|
||||
|
||||
@point.setter
|
||||
def point(self, point):
|
||||
self._point = point
|
||||
|
||||
def point_on_surface(self, point):
|
||||
"""Returns ``True`` if a point is on the sphere's surface.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
point : Point
|
||||
The point for which it's to be ascertained if it's on the sphere's
|
||||
surface or not. This point's position relative to the sphere's
|
||||
center must be a simple expression involving the radius of the
|
||||
sphere, otherwise this check will likely not work.
|
||||
|
||||
"""
|
||||
point_vector = point.pos_from(self.point)
|
||||
if isinstance(point_vector, Vector):
|
||||
point_radius_squared = dot(point_vector, point_vector)
|
||||
else:
|
||||
point_radius_squared = point_vector**2
|
||||
return Eq(point_radius_squared, self.radius**2) == True
|
||||
|
||||
def geodesic_length(self, point_1, point_2):
|
||||
r"""Returns the shortest distance between two points on the sphere's
|
||||
surface.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
The geodesic length, i.e. the shortest arc along the surface of a
|
||||
sphere, connecting two points can be calculated using the formula:
|
||||
|
||||
.. math::
|
||||
|
||||
l = \arccos\left(\mathbf{v}_1 \cdot \mathbf{v}_2\right)
|
||||
|
||||
where $\mathbf{v}_1$ and $\mathbf{v}_2$ are the unit vectors from the
|
||||
sphere's center to the first and second points on the sphere's surface
|
||||
respectively. Note that the actual path that the geodesic will take is
|
||||
undefined when the two points are directly opposite one another.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
A geodesic length can only be calculated between two points on the
|
||||
sphere's surface. Firstly, a ``WrappingSphere`` instance must be
|
||||
created along with two points that will lie on its surface:
|
||||
|
||||
>>> from sympy import symbols
|
||||
>>> from sympy.physics.mechanics import (Point, ReferenceFrame,
|
||||
... WrappingSphere)
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> r = symbols('r')
|
||||
>>> pO = Point('pO')
|
||||
>>> pO.set_vel(N, 0)
|
||||
>>> sphere = WrappingSphere(r, pO)
|
||||
>>> p1 = Point('p1')
|
||||
>>> p2 = Point('p2')
|
||||
|
||||
Let's assume that ``p1`` lies at a distance of ``r`` in the ``N.x``
|
||||
direction from ``pO`` and that ``p2`` is located on the sphere's
|
||||
surface in the ``N.y + N.z`` direction from ``pO``. These positions can
|
||||
be set with:
|
||||
|
||||
>>> p1.set_pos(pO, r*N.x)
|
||||
>>> p1.pos_from(pO)
|
||||
r*N.x
|
||||
>>> p2.set_pos(pO, r*(N.y + N.z).normalize())
|
||||
>>> p2.pos_from(pO)
|
||||
sqrt(2)*r/2*N.y + sqrt(2)*r/2*N.z
|
||||
|
||||
The geodesic length, which is in this case is a quarter of the sphere's
|
||||
circumference, can be calculated using the ``geodesic_length`` method:
|
||||
|
||||
>>> sphere.geodesic_length(p1, p2)
|
||||
pi*r/2
|
||||
|
||||
If the ``geodesic_length`` method is passed an argument, the ``Point``
|
||||
that doesn't lie on the sphere's surface then a ``ValueError`` is
|
||||
raised because it's not possible to calculate a value in this case.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
point_1 : Point
|
||||
Point from which the geodesic length should be calculated.
|
||||
point_2 : Point
|
||||
Point to which the geodesic length should be calculated.
|
||||
|
||||
"""
|
||||
for point in (point_1, point_2):
|
||||
if not self.point_on_surface(point):
|
||||
msg = (
|
||||
f'Geodesic length cannot be calculated as point {point} '
|
||||
f'with radius {point.pos_from(self.point).magnitude()} '
|
||||
f'from the sphere\'s center {self.point} does not lie on '
|
||||
f'the surface of {self} with radius {self.radius}.'
|
||||
)
|
||||
raise ValueError(msg)
|
||||
point_1_vector = point_1.pos_from(self.point).normalize()
|
||||
point_2_vector = point_2.pos_from(self.point).normalize()
|
||||
central_angle = acos(point_2_vector.dot(point_1_vector))
|
||||
geodesic_length = self.radius*central_angle
|
||||
return geodesic_length
|
||||
|
||||
def geodesic_end_vectors(self, point_1, point_2):
|
||||
"""The vectors parallel to the geodesic at the two end points.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
point_1 : Point
|
||||
The point from which the geodesic originates.
|
||||
point_2 : Point
|
||||
The point at which the geodesic terminates.
|
||||
|
||||
"""
|
||||
pA, pB = point_1, point_2
|
||||
pO = self.point
|
||||
pA_vec = pA.pos_from(pO)
|
||||
pB_vec = pB.pos_from(pO)
|
||||
|
||||
if pA_vec.cross(pB_vec) == 0:
|
||||
msg = (
|
||||
f'Can\'t compute geodesic end vectors for the pair of points '
|
||||
f'{pA} and {pB} on a sphere {self} as they are diametrically '
|
||||
f'opposed, thus the geodesic is not defined.'
|
||||
)
|
||||
raise ValueError(msg)
|
||||
|
||||
return (
|
||||
pA_vec.cross(pB.pos_from(pA)).cross(pA_vec).normalize(),
|
||||
pB_vec.cross(pA.pos_from(pB)).cross(pB_vec).normalize(),
|
||||
)
|
||||
|
||||
def __repr__(self):
|
||||
"""Representation of a ``WrappingSphere``."""
|
||||
return (
|
||||
f'{self.__class__.__name__}(radius={self.radius}, '
|
||||
f'point={self.point})'
|
||||
)
|
||||
|
||||
|
||||
class WrappingCylinder(WrappingGeometryBase):
|
||||
"""A solid (infinite) cylindrical object.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
A wrapping geometry that allows for circular arcs to be defined between
|
||||
pairs of points. These paths are always geodetic (the shortest possible) in
|
||||
the sense that they will be a straight line on the unwrapped cylinder's
|
||||
surface. However, it is also possible for a direction to be specified, i.e.
|
||||
paths can be influenced such that they either wrap along the shortest side
|
||||
or the longest side of the cylinder. To define these directions, rotations
|
||||
are in the positive direction following the right-hand rule.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
To create a ``WrappingCylinder`` instance, a ``Symbol`` denoting its
|
||||
radius, a ``Vector`` defining its axis, and a ``Point`` through which its
|
||||
axis passes are needed:
|
||||
|
||||
>>> from sympy import symbols
|
||||
>>> from sympy.physics.mechanics import (Point, ReferenceFrame,
|
||||
... WrappingCylinder)
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> r = symbols('r')
|
||||
>>> pO = Point('pO')
|
||||
>>> ax = N.x
|
||||
|
||||
A cylinder with radius ``r``, and axis parallel to ``N.x`` passing through
|
||||
``pO`` can be instantiated with:
|
||||
|
||||
>>> WrappingCylinder(r, pO, ax)
|
||||
WrappingCylinder(radius=r, point=pO, axis=N.x)
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
radius : Symbol
|
||||
The radius of the cylinder.
|
||||
point : Point
|
||||
A point through which the cylinder's axis passes.
|
||||
axis : Vector
|
||||
The axis along which the cylinder is aligned.
|
||||
|
||||
See Also
|
||||
========
|
||||
|
||||
WrappingSphere: Spherical geometry where the wrapping direction is always
|
||||
geodetic.
|
||||
|
||||
"""
|
||||
|
||||
def __init__(self, radius, point, axis):
|
||||
"""Initializer for ``WrappingCylinder``.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
radius : Symbol
|
||||
The radius of the cylinder. This symbol must represent a value that
|
||||
is positive and constant, i.e. it cannot be a dynamic symbol.
|
||||
point : Point
|
||||
A point through which the cylinder's axis passes.
|
||||
axis : Vector
|
||||
The axis along which the cylinder is aligned.
|
||||
|
||||
"""
|
||||
self.radius = radius
|
||||
self.point = point
|
||||
self.axis = axis
|
||||
|
||||
@property
|
||||
def radius(self):
|
||||
"""Radius of the cylinder."""
|
||||
return self._radius
|
||||
|
||||
@radius.setter
|
||||
def radius(self, radius):
|
||||
self._radius = radius
|
||||
|
||||
@property
|
||||
def point(self):
|
||||
"""A point through which the cylinder's axis passes."""
|
||||
return self._point
|
||||
|
||||
@point.setter
|
||||
def point(self, point):
|
||||
self._point = point
|
||||
|
||||
@property
|
||||
def axis(self):
|
||||
"""Axis along which the cylinder is aligned."""
|
||||
return self._axis
|
||||
|
||||
@axis.setter
|
||||
def axis(self, axis):
|
||||
self._axis = axis.normalize()
|
||||
|
||||
def point_on_surface(self, point):
|
||||
"""Returns ``True`` if a point is on the cylinder's surface.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
point : Point
|
||||
The point for which it's to be ascertained if it's on the
|
||||
cylinder's surface or not. This point's position relative to the
|
||||
cylinder's axis must be a simple expression involving the radius of
|
||||
the sphere, otherwise this check will likely not work.
|
||||
|
||||
"""
|
||||
relative_position = point.pos_from(self.point)
|
||||
parallel = relative_position.dot(self.axis) * self.axis
|
||||
point_vector = relative_position - parallel
|
||||
if isinstance(point_vector, Vector):
|
||||
point_radius_squared = dot(point_vector, point_vector)
|
||||
else:
|
||||
point_radius_squared = point_vector**2
|
||||
return Eq(trigsimp(point_radius_squared), self.radius**2) == True
|
||||
|
||||
def geodesic_length(self, point_1, point_2):
|
||||
"""The shortest distance between two points on a geometry's surface.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
The geodesic length, i.e. the shortest arc along the surface of a
|
||||
cylinder, connecting two points. It can be calculated using Pythagoras'
|
||||
theorem. The first short side is the distance between the two points on
|
||||
the cylinder's surface parallel to the cylinder's axis. The second
|
||||
short side is the arc of a circle between the two points of the
|
||||
cylinder's surface perpendicular to the cylinder's axis. The resulting
|
||||
hypotenuse is the geodesic length.
|
||||
|
||||
Examples
|
||||
========
|
||||
|
||||
A geodesic length can only be calculated between two points on the
|
||||
cylinder's surface. Firstly, a ``WrappingCylinder`` instance must be
|
||||
created along with two points that will lie on its surface:
|
||||
|
||||
>>> from sympy import symbols, cos, sin
|
||||
>>> from sympy.physics.mechanics import (Point, ReferenceFrame,
|
||||
... WrappingCylinder, dynamicsymbols)
|
||||
>>> N = ReferenceFrame('N')
|
||||
>>> r = symbols('r')
|
||||
>>> pO = Point('pO')
|
||||
>>> pO.set_vel(N, 0)
|
||||
>>> cylinder = WrappingCylinder(r, pO, N.x)
|
||||
>>> p1 = Point('p1')
|
||||
>>> p2 = Point('p2')
|
||||
|
||||
Let's assume that ``p1`` is located at ``N.x + r*N.y`` relative to
|
||||
``pO`` and that ``p2`` is located at ``r*(cos(q)*N.y + sin(q)*N.z)``
|
||||
relative to ``pO``, where ``q(t)`` is a generalized coordinate
|
||||
specifying the angle rotated around the ``N.x`` axis according to the
|
||||
right-hand rule where ``N.y`` is zero. These positions can be set with:
|
||||
|
||||
>>> q = dynamicsymbols('q')
|
||||
>>> p1.set_pos(pO, N.x + r*N.y)
|
||||
>>> p1.pos_from(pO)
|
||||
N.x + r*N.y
|
||||
>>> p2.set_pos(pO, r*(cos(q)*N.y + sin(q)*N.z).normalize())
|
||||
>>> p2.pos_from(pO).simplify()
|
||||
r*cos(q(t))*N.y + r*sin(q(t))*N.z
|
||||
|
||||
The geodesic length, which is in this case a is the hypotenuse of a
|
||||
right triangle where the other two side lengths are ``1`` (parallel to
|
||||
the cylinder's axis) and ``r*q(t)`` (parallel to the cylinder's cross
|
||||
section), can be calculated using the ``geodesic_length`` method:
|
||||
|
||||
>>> cylinder.geodesic_length(p1, p2).simplify()
|
||||
sqrt(r**2*q(t)**2 + 1)
|
||||
|
||||
If the ``geodesic_length`` method is passed an argument ``Point`` that
|
||||
doesn't lie on the sphere's surface then a ``ValueError`` is raised
|
||||
because it's not possible to calculate a value in this case.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
point_1 : Point
|
||||
Point from which the geodesic length should be calculated.
|
||||
point_2 : Point
|
||||
Point to which the geodesic length should be calculated.
|
||||
|
||||
"""
|
||||
for point in (point_1, point_2):
|
||||
if not self.point_on_surface(point):
|
||||
msg = (
|
||||
f'Geodesic length cannot be calculated as point {point} '
|
||||
f'with radius {point.pos_from(self.point).magnitude()} '
|
||||
f'from the cylinder\'s center {self.point} does not lie on '
|
||||
f'the surface of {self} with radius {self.radius} and axis '
|
||||
f'{self.axis}.'
|
||||
)
|
||||
raise ValueError(msg)
|
||||
|
||||
relative_position = point_2.pos_from(point_1)
|
||||
parallel_length = relative_position.dot(self.axis)
|
||||
|
||||
point_1_relative_position = point_1.pos_from(self.point)
|
||||
point_1_perpendicular_vector = (
|
||||
point_1_relative_position
|
||||
- point_1_relative_position.dot(self.axis)*self.axis
|
||||
).normalize()
|
||||
|
||||
point_2_relative_position = point_2.pos_from(self.point)
|
||||
point_2_perpendicular_vector = (
|
||||
point_2_relative_position
|
||||
- point_2_relative_position.dot(self.axis)*self.axis
|
||||
).normalize()
|
||||
|
||||
central_angle = _directional_atan(
|
||||
cancel(point_1_perpendicular_vector
|
||||
.cross(point_2_perpendicular_vector)
|
||||
.dot(self.axis)),
|
||||
cancel(point_1_perpendicular_vector.dot(point_2_perpendicular_vector)),
|
||||
)
|
||||
|
||||
planar_arc_length = self.radius*central_angle
|
||||
geodesic_length = sqrt(parallel_length**2 + planar_arc_length**2)
|
||||
return geodesic_length
|
||||
|
||||
def geodesic_end_vectors(self, point_1, point_2):
|
||||
"""The vectors parallel to the geodesic at the two end points.
|
||||
|
||||
Parameters
|
||||
==========
|
||||
|
||||
point_1 : Point
|
||||
The point from which the geodesic originates.
|
||||
point_2 : Point
|
||||
The point at which the geodesic terminates.
|
||||
|
||||
"""
|
||||
point_1_from_origin_point = point_1.pos_from(self.point)
|
||||
point_2_from_origin_point = point_2.pos_from(self.point)
|
||||
|
||||
if point_1_from_origin_point == point_2_from_origin_point:
|
||||
msg = (
|
||||
f'Cannot compute geodesic end vectors for coincident points '
|
||||
f'{point_1} and {point_2} as no geodesic exists.'
|
||||
)
|
||||
raise ValueError(msg)
|
||||
|
||||
point_1_parallel = point_1_from_origin_point.dot(self.axis) * self.axis
|
||||
point_2_parallel = point_2_from_origin_point.dot(self.axis) * self.axis
|
||||
point_1_normal = (point_1_from_origin_point - point_1_parallel)
|
||||
point_2_normal = (point_2_from_origin_point - point_2_parallel)
|
||||
|
||||
if point_1_normal == point_2_normal:
|
||||
point_1_perpendicular = Vector(0)
|
||||
point_2_perpendicular = Vector(0)
|
||||
else:
|
||||
point_1_perpendicular = self.axis.cross(point_1_normal).normalize()
|
||||
point_2_perpendicular = -self.axis.cross(point_2_normal).normalize()
|
||||
|
||||
geodesic_length = self.geodesic_length(point_1, point_2)
|
||||
relative_position = point_2.pos_from(point_1)
|
||||
parallel_length = relative_position.dot(self.axis)
|
||||
planar_arc_length = sqrt(geodesic_length**2 - parallel_length**2)
|
||||
|
||||
point_1_vector = (
|
||||
planar_arc_length * point_1_perpendicular
|
||||
+ parallel_length * self.axis
|
||||
).normalize()
|
||||
point_2_vector = (
|
||||
planar_arc_length * point_2_perpendicular
|
||||
- parallel_length * self.axis
|
||||
).normalize()
|
||||
|
||||
return (point_1_vector, point_2_vector)
|
||||
|
||||
def __repr__(self):
|
||||
"""Representation of a ``WrappingCylinder``."""
|
||||
return (
|
||||
f'{self.__class__.__name__}(radius={self.radius}, '
|
||||
f'point={self.point}, axis={self.axis})'
|
||||
)
|
||||
|
||||
|
||||
def _directional_atan(numerator, denominator):
|
||||
"""Compute atan in a directional sense as required for geodesics.
|
||||
|
||||
Explanation
|
||||
===========
|
||||
|
||||
To be able to control the direction of the geodesic length along the
|
||||
surface of a cylinder a dedicated arctangent function is needed that
|
||||
properly handles the directionality of different case. This function
|
||||
ensures that the central angle is always positive but shifting the case
|
||||
where ``atan2`` would return a negative angle to be centered around
|
||||
``2*pi``.
|
||||
|
||||
Notes
|
||||
=====
|
||||
|
||||
This function only handles very specific cases, i.e. the ones that are
|
||||
expected to be encountered when calculating symbolic geodesics on uniformly
|
||||
curved surfaces. As such, ``NotImplemented`` errors can be raised in many
|
||||
cases. This function is named with a leader underscore to indicate that it
|
||||
only aims to provide very specific functionality within the private scope
|
||||
of this module.
|
||||
|
||||
"""
|
||||
|
||||
if numerator.is_number and denominator.is_number:
|
||||
angle = atan2(numerator, denominator)
|
||||
if angle < 0:
|
||||
angle += 2 * pi
|
||||
elif numerator.is_number:
|
||||
msg = (
|
||||
f'Cannot compute a directional atan when the numerator {numerator} '
|
||||
f'is numeric and the denominator {denominator} is symbolic.'
|
||||
)
|
||||
raise NotImplementedError(msg)
|
||||
elif denominator.is_number:
|
||||
msg = (
|
||||
f'Cannot compute a directional atan when the numerator {numerator} '
|
||||
f'is symbolic and the denominator {denominator} is numeric.'
|
||||
)
|
||||
raise NotImplementedError(msg)
|
||||
else:
|
||||
ratio = sympify(trigsimp(numerator / denominator))
|
||||
if isinstance(ratio, tan):
|
||||
angle = ratio.args[0]
|
||||
elif (
|
||||
ratio.is_Mul
|
||||
and ratio.args[0] == Integer(-1)
|
||||
and isinstance(ratio.args[1], tan)
|
||||
):
|
||||
angle = 2 * pi - ratio.args[1].args[0]
|
||||
else:
|
||||
msg = f'Cannot compute a directional atan for the value {ratio}.'
|
||||
raise NotImplementedError(msg)
|
||||
|
||||
return angle
|
||||
Reference in New Issue
Block a user