.. _trep_system:
:class:`System` - A Mechanical System
=====================================
.. currentmodule:: trep
The :class:`System` object is the central component for modeling a
mechanical system, and usually the first :mod:`trep` object you
create. It contains the entire definition of the system, including
all coordinate frames, configuration variables, constraints, etc.
:class:`System` is responsible for calculating continuous dynamics and
derivative, but it is also used for the underlying Lagrangian
calculations in any discrete dynamics calculations.
A :class:`System` has a single inertial coordinate frame, accessible
through the :attr:`world_frame` attribute. Every other coordinate
frame added to the system will be descended from this coordinate
frame.
.. class:: System()
Create a new empty mechanical system.
.. contents::
:local:
System Components
-----------------
.. attribute:: System.world_frame
The root spatial frame of the system. The world frame will always
exist and cannot be changed other than adding child frames.
*(read only)*
.. attribute:: System.frames
System.configs
System.dyn_configs
System.kin_configs
System.potentials
System.forces
System.inputs
System.constraints
Tuples of all the components in the system. **The order of
components in these tuples defines their order throughout**
:mod:`trep`. Any vector of configuration values will be the same
order as :attr:`System.configs`, any vector of constraint forces
will be the same order as :attr:`System.constraints`, etc.
For example, any array of numbers representing a configuration will
be ordered according to :attr:`System.configs`. An array of
constraint forces will correspond to the order of
:attr:`System.constraints`.
:attr:`configs` is guaranteed to be ordered as the concatenation of
:attr:`dyn_configs` and :attr:`kin_configs`::
System.configs = System.dyn_configs + System.kin_configs
The tuples are read only, but the components in them may be
modified.
.. attribute:: System.masses
A tuple of all the :class:`Frame` objects in the system with
non-zero mass properties.
*(read only)*
.. attribute:: System.nQ
Number of configuration variables in the system. Equivalent to
``len(system.configs)``.
*(read only)*
.. attribute:: System.nQd
Number of dynamic configuration variables in the system. Equivalent
to ``len(system.dyn_configs)``.
*(read only)*
.. attribute:: System.nQk
Number of kinematic configuration variables in the system.
Equivalent to ``len(system.kin_configs)``.
*(read only)*
.. attribute:: System.nu
Number of inputs in the system.
Equivalent to ``len(system.inputs)``.
*(read only)*
.. attribute:: System.nc
Number of constraints in the system.
Equivalent to ``len(system.constraints)``.
*(read only)*
.. _finding_specific_components:
Finding Specific Components
---------------------------
.. method:: System.get_frame(identifier)
System.get_config(identifier)
System.get_potential(identifier)
System.get_constraint(identifier)
System.get_force(identifier)
System.get_input(identifier)
Find a specific component in the system. The identifier can be:
- Integer: Returns the component at that position in the
corresponding tuple.
system.get_frame(i) = system.frames[i]
If the index is invalid, an :exc:`IndexError` exception is
raised.
- String: Returns the component with the matching name:
system.get_config('theta').name == 'theta'
If no object has a matching name, a :exc:`KeyError` exception is
raised.
- Object: Return the identifier unmodified::
system.get_force(force) == force
If the object is the incorrect type, a :exc:`TypeError` exception
is raised::
>>> config = system.configs[0]
>>> system.get_frame(config)
Traceback (most recent call last):
File "<stdin>", line 1, in <module>
File "/usr/local/lib/python2.7/dist-packages/trep/system.py", line 106, in get_frame
return self._get_object(identifier, Frame, self.frames)
File "/usr/local/lib/python2.7/dist-packages/trep/system.py", line 509, in _get_object
raise TypeError()
TypeError
Importing and Exporting Frame Definitions
-----------------------------------------
.. method:: System.import_frames(children)
Adds children to this system's world frame using a special
frame definition. See :meth:`Frame.import_frames` for details.
.. method:: System.export_frames(system_name='system', frames_name='frames', tab_size=4)
Create python source code to define this system's frames. The code
is returned as a string.
System State
------------
A :class:`System` is a stateful object that has a current time,
configuration, velocity, and input at all times. When working
directly with a :class:`System`, you are a responsible for setting the
state.
.. attribute:: System.t
Current time of the system.
The other state information is actually spread out among each
component in the system. For example, each current configuration
value is stored in :attr:`Config.q`. These can be modified directly
through each component, but the following attributes are usually more
convenient for reading and writing multiple values at once.
.. attribute:: System.q
System.dq
System.ddq
The value, velocity, and acceleration of the complete
configuration.
.. attribute:: System.qd
System.dqd
System.ddqd
The value, velocity, and acceleration of the dynamic configuration.
.. attribute:: System.qk
System.dqk
System.ddqk
The value, velocity, and acceleration of the kinematic
configuration.
.. attribute:: System.u
The current values of the force inputs.
Reading the each attribute will return a numpy array of the current
values.
The values can be set with three different methods:
- A dictionary that maps names to values::
>>> system.q = { 'x' : 1.0, 'theta' : 0.1}
Any variables that are not named will be unchanged.
- A array-like list of numbers::
>>> system.q = [1.0, 0.1, 0.0, 2.3]
>>> system.q = np.array([1.0, 0.1, 0.0, 2.3])
If the size of the array and the number of variables doesn't
match, the shorter of the two will be used::
>>> system.nQ
4
# This only sets the first 2 configuration variables!
>>> system.q = [0.2, 5.0]
# This ignores the last 2 values!
>>> system.q = [0.5, 0, 1.1, 2.4, 9.0]
- A single number::
>>> system.q = 0
This will set the entire configuration to the number.
.. method:: System.set_state(q=None, dq=None, u=None, ddqk=None, t=None)
Set the current state of the system, not including the "output"
ddqd. The types of values accepted are the same as described
above.
Constraints
-----------
.. method:: System.satisfy_constraints(tolerance=1e-10, verbose=False, \
keep_kinematic=False, constant_q_list=None)
Modify the current configuration to satisfy the system constraints.
Letting :math:`q_0` be the system's current configuration, this
performs the optimization:
.. math::
q = \arg\min_q |q-q_0|^2 \quad \mathrm{s.t.}\quad h(q) = 0
The new configuration will be set in the system and returned. If
the optimization fails, a :exc:`StandardError` exception is raised.
Setting *verbose* to :data:`True` will make the optimization print
out information to the console while it is running.
Setting *keep_kinematic* to :data:`True` will modify above optimization
to optimize over :math:`q_d` instead of :math:`q`; thus all kinematic
configuration variables will be kept constant. Passing a list (or
tuple) of configurations to *constant_q_list* will define an
arbitrary set of configurations to hold constant during
optimization. Internally this method uses
:meth:`System.get_config` thus, any valid *identifier* mentioned
in :ref:`Finding Specific Components
<finding_specific_components>` section will work.
.. method:: System.minimize_potential_energy(tolerance=1e-10, verbose=False, \
keep_kinematic=False, constant_q_list=None)
Modify the current configuration to satisfy the system constraints while also
minimizing potential energy. The system velocity is set to zero, and thus
the kinetic energy is zero. Therefore, the actual optimization that is
solved is given by
.. math::
q = \arg\min_q \; (-L(q,\dot{q}=0))\quad \mathrm{s.t.}\quad h(q) = 0
The new configuration will be set in the system and returned. If
the optimization fails, a :exc:`StandardError` exception is raised. This
function may be useful for finding nearby equilibrium points that
satisfy the system constraints.
Setting *verbose* to :data:`True` will make the optimization print
out information to the console while it is running.
Setting *keep_kinematic* to :data:`True` will modify above optimization
to optimize over :math:`q_d` instead of :math:`q`; thus all kinematic
configuration variables will be kept constant. Passing a list (or
tuple) of configurations to *constant_q_list* will define an
arbitrary set of configurations to hold constant during
optimization. Internally this method uses
:meth:`System.get_config` thus, any valid *identifier* mentioned
in :ref:`Finding Specific Components
<finding_specific_components>` section will work.
Lagrangian Calculations
-----------------------
.. method:: System.total_energy()
Calculate the total energy in the current state.
.. method:: System.L()
System.L_dq(q1)
System.L_dqdq(q1, q2)
System.L_dqdqdq(q1, q2, q3)
System.L_ddq(dq1)
System.L_ddqdq(dq1, q2)
System.L_ddqdqdq(dq1, q2, q3)
System.L_ddqdqdqdq(dq1, q2, q3, q4)
System.L_ddqddq(dq1, dq2)
System.L_ddqddqdq(dq1, dq2, q3)
System.L_ddqddqdqdq(dq1, dq2, q3, q4)
Calculate the Lagrangian or it's derivatives at the current state.
When calculating the Lagrangian derivatives, you must specify a
variable to take the derivative with respect to.
Calculating :math:`\deriv[L]{q_0}`::
>>> q0 = system.configs[0]
>>> system.L_dq(q0)
Calculating :math:`\derivII[L]{q_0}{\dq_1}`::
>>> q0 = system.configs[0]
>>> q1 = system.configs[1]
>>> system.L_ddqdq(q1, q0)
Calculating :math:`\derivII[L]{\dq_0}{\dq_1}`::
>>> q0 = system.configs[0]
>>> q1 = system.configs[1]
>>> system.L_ddqddq(q1, q0)
# Mixed partials always commute, so we could also do:
>>> system.L_ddqddq(q0, q1)
Calculate an entire derivative :math:`\deriv[L]{q}`::
>>> [system.L_dq(q) for q in system.configs]
.. _system_dynamics_functions:
Dynamics
--------
.. method:: System.f(q=None)
:type q: :class:`Config` or :data:`None`
Calculate the dynamics at the current state, :math:`\ddq_d = f(q,\dq,\ddq_k, u, t)`.
This calculates the second derivative of the dynamic configuration
variables. The results are also written to :attr:`Config.ddq`.
If *q* is :data:`None`, the entire vector :math:`\ddq_d` is
returned. The array is in the same order as
:attr:`System.dyn_configs`.
If *q* is specified, it must be a dynamic configuration variable,
and it's second derivative will be returned. This could also be
accessed as ``q.ddq``.
Once the dynamics are calculated, the results are saved until the
system's state changes, so repeated calls will not keep repeating
work.
.. method:: System.f_dq(q=None, q1=None)
System.f_ddq(q=None, dq1=None)
System.f_dddk(q=None, k1=None)
System.f_du(q=None, u1=None)
Calculate the first derivative of the dynamics with respect to the
configuration, the configuration velocity, the kinematic
acceleration, or the force inputs.
If both parameters are :data:`None`, the entire first derivative is
returned as a :mod:`numpy` array with the derivatives across the
rows.
If any parameters are specified, they must be appropriate objects,
and the function will return the specific information requested.
Calculating :math:`\deriv[f]{q_2}`::
>>> dq = system.configs[2]
>>> system.f_dq(q1=dq)
array([-0.076, -0.018, -0.03 , ..., 0.337, -0.098, 0.562])
Calculating :math:`\deriv[f]{\dq}`::
>>> system.f_ddq()
array([[-0.001, -0. , -0. , ..., 0.001, -0.001, 0. ],
[-0. , -0.002, -0. , ..., -0. , -0. , 0. ],
[-0. , -0. , -0. , ..., 0. , -0. , 0. ],
...,
[-0. , -0.001, -0. , ..., -0.003, -0. , 0. ],
[ 0.001, -0. , 0. , ..., -0.005, 0.008, -0. ],
[-0.001, -0. , -0. , ..., 0.008, -0.027, 0. ]])
>>> system.f_ddq().shape
(22, 23)
Calculating :math:`\deriv[\ddq_4]{\ddq_{k0}}`::
>>> qk = system.kin_configs[0]
>>> q = system.dyn_configs[4]
>>> system.f_dddk(q, qk)
-0.31036045452513322
The first call to any of these functions will calculate and cache
the entire first derivative. Once calculated, subsequent calls
will not recalculate the derivatives until the system's state is
changed.
.. method:: System.f_dqdq(q=None, q1=None, q2=None)
System.f_ddqdq(q=None, dq1=None, q2=None)
System.f_ddqddq(q=None, dq1=None, dq2=None)
System.f_dddkdq(q=None, k1=None, q2=None)
System.f_dudq(q=None, u1=None, q2=None)
System.f_duddq(q=None, u1=None, dq2=None)
System.f_dudu(q=None, u1=None, u2=None)
Calculate second derivatives of the dynamics.
If no parameters specified, the entire second derivative is
returned as a :mod:`numpy` array. The returned arrays are indexed
with the two derivatives variables as the first two dimensions and
the dynamic acceleration as the last dimension. In other words,
calling::
system.f_dqdq(q, q1, q2)
is equivalent to::
result = system.f_dqdq()
result[q1.index, q2.index, q.index]
If any parameters are specified, they must be appropriate objects,
and the function will return the specific information requested.
For example::
system.f_dqdq(q2=q2)
is equivalent to::
result = system.f_dqdq()
result[:, q2.index, :]
The second derivatives are indexed opposite from the first
derivatives because they are generally multiplied by an adjoint
variable in practice. For example, the quantity:
.. math::
z^T \derivII[f]{q}{\dq}
can be calculated as::
numpy.inner(system.f_dqddq(), z)
without having to do a transpose or specify a specific axis.
The first call to any of these functions will calculate and cache
the entire second derivative. Once calculated, subsequent calls
will not recalculate the derivatives until the system's state is
changed.
Constraint Forces
-----------------
.. method:: System.lambda_(constraint=None)
.. method:: System.lambda_dq(constraint=None, q1=None)
System.lambda_ddq(constraint=None, dq1=None)
System.lambda_dddk(constraint=None, k1=None)
System.lambda_du(constraint=None, u1=None)
.. method:: System.lambda_dqdq(constraint=None, q1=None, q2=None)
System.lambda_ddqdq(constraint=None, dq1=None, q2=None)
System.lambda_ddqddq(constraint=None, dq1=None, dq2=None)
System.lambda_dddkdq(constraint=None, k1=None, q2=None)
System.lambda_dudq(constraint=None, u1=None, q2=None)
System.lambda_duddq(constraint=None, u1=None, dq2=None)
System.lambda_dudu(constraint=None, u1=None, u2=None)
These are functions for calculating the value of the constraint
force vector :math:`\lambda` and its derivatives. They have the
same behavior and index orders as the :ref:`dynamics <system_dynamics_functions>` functions.
Derivative Testing
------------------
.. method:: System.test_derivative_dq(func, func_dq, delta=1e-6, tolerance=1e-7, verbose=False, \
test_name='<unnamed>')
System.test_derivative_ddq(func, func_ddq, delta=1e-6, tolerance=1e-7, \
verbose=False, test_name='<unnamed>')
Test the derivative of a function with respect to a
configuration variable's time derivative and its numerical
approximation.
func -> Callable taking no arguments and returning float or np.array
func_ddq -> Callable taking one configuration variable argument
and returning a float or np.array.
delta -> perturbation to the current configuration to
calculate the numeric approximation.
tolerance -> acceptable difference between the approximation
and exact value. (\|exact - approx\| <= tolerance)
verbose -> Boolean indicating if a message should be printed for failures.
name -> String identifier to print out when reporting messages
when verbose is true.
Returns False if any tests fail and True otherwise.
Structure Updates
-----------------
Whenever a system is significantly modified, an internal function is
called to make sure everything is consistent and resize the arrays
used for calculating values as needed. You can register a function to
be called after the system has been made consistent using the
:meth:`add_structure_changed_func`. This is useful if you building
your own component that needs to be updated whenever the system's
structure changed (See :class:`Damping` for an example).
.. method:: System.add_structure_changed_func(function)
Register a function to call whenever the system structure changes.
This includes adding and removing frames, configuration variables,
constraints, potentials, and forces.
Since every addition to the system triggers a structure update,
building a large system can cause a long delay. In these cases, it is
useful to stop the structure updates until the system is fully
constructed and then perform the update once.
.. warning::
Be sure to remove all holds before performing any calculations with system.
.. method:: System.hold_structure_changes()
Prevent the system from calling :meth:`System._update_structure()`
(mostly). This can be called multiple times, but
:meth:`resume_structure_changes` must be called an equal number of
times.
.. method:: System.resume_structure_changes()
Stop preventing the system from calling
:meth:`System._update_structure()`. The structure will only be
updated once every hold has been removed, so calling this does not
guarantee that the structure will be immediately update.