chore: 添加虚拟环境到仓库
- 添加 backend_service/venv 虚拟环境 - 包含所有Python依赖包 - 注意：虚拟环境约393MB，包含12655个文件
This commit is contained in:
@@ -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)
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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)
|
||||
@@ -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
|
||||
@@ -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)
|
||||
@@ -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__
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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)
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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.')
|
||||
@@ -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.")
|
||||
@@ -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
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -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
|
||||
@@ -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)]
|
||||
@@ -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)
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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))
|
||||
@@ -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)
|
||||
@@ -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()))
|
||||
@@ -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)
|
||||
@@ -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.
|
||||
@@ -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))
|
||||
@@ -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)
|
||||
@@ -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
|
||||
@@ -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)
|
||||
@@ -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
|
||||
@@ -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]))
|
||||
@@ -0,0 +1,5 @@
|
||||
from sympy.physics.mechanics.method import _Methods
|
||||
from sympy.testing.pytest import raises
|
||||
|
||||
def test_method():
|
||||
raises(TypeError, lambda: _Methods())
|
||||
@@ -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])
|
||||
@@ -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)
|
||||
@@ -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
|
||||
@@ -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)
|
||||
@@ -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
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
@@ -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