From 22f8456591cc8f8f8ee1a58ec5272a2c65aa68a5 Mon Sep 17 00:00:00 2001 From: Keenon Werling Date: Mon, 2 Dec 2024 09:41:24 -0800 Subject: [PATCH] Updating the docs --- www/docs/equations-of-motion.rst | 51 ++++++++++++++++++++++++++++++++ www/docs/inverse-dynamics.rst | 10 +++---- 2 files changed, 56 insertions(+), 5 deletions(-) create mode 100644 www/docs/equations-of-motion.rst diff --git a/www/docs/equations-of-motion.rst b/www/docs/equations-of-motion.rst new file mode 100644 index 000000000..69839a92d --- /dev/null +++ b/www/docs/equations-of-motion.rst @@ -0,0 +1,51 @@ +Manual Equations of Motion +========================================== + +Often, it is useful to be able to peer inside the black box of a simulation and see how the sausage is made. +This section will provide a brief overview of the equations of motion that are used in the simulation, +and how to access the quantities you need in order to recreate the simulation results manually with Numpy. + +The equations of motion are derived from the Lagrangian formulation of classical mechanics. For details +on how these equations are implemented at a low level, see the `Nimble Wiki `_. +This in turn is based on the `GEAR Notes `_. + +At its heart, we are generally using the following dynamics equation: + +:math:`M(q) \ddot{q} + C(q, \dot{q}) = \tau` + +Or rearranging for forward dynamics: + +:math:`\ddot{q} = M(q)^{-1} (\tau - C(q, \dot{q}))` + +Where: + +- :math:`q` is the generalized coordinates of the system, which can be got with :code:`skeleton.getPositions()` or :code:`world.getPositions()` +- :math:`\dot{q}` is the generalized velocities of the system, which can be got with :code:`skeleton.getVelocities()` or :code:`world.getVelocities()` +- :math:`\ddot{q}` is the generalized accelerations of the system, which can be got with :code:`skeleton.getAccelerations()` or :code:`world.getAccelerations()` +- :math:`M(q)` is the mass matrix of the system, which can be got with :code:`skeleton.getMassMatrix()` or :code:`world.getMassMatrix()` +- :math:`C(q, \dot{q})` is the Coriolis and centrifugal forces of the system, which can be got with :code:`skeleton.getCoriolisAndGravityForces()` or :code:`world.getCoriolisAndGravityForces()` + +To put this into code:: + + import numpy as np + import nimblephysics as nimble + + rajagopal_opensim: nimble.biomechanics.OpenSimFile = nimble.RajagopalHumanBodyModel() + skeleton: nimble.dynamics.Skeleton = rajagopal_opensim.skeleton + + # Set the generalized coordinates, velocities, and accelerations + q = skeleton.getPositions() + dq = skeleton.getVelocities() + ddq = skeleton.getAccelerations() + + # Get the mass matrix + M = skeleton.getMassMatrix() + + # Get the Coriolis and centrifugal forces + C = skeleton.getCoriolisAndGravityForces() - skeleton.getExternalForces() + skeleton.getDampingForce() + skeleton.getSpringForce() + + # Get the generalized forces + tau = skeleton.getForces() + + # Calculate the accelerations + ddq = np.linalg.solve(M, tau - C) diff --git a/www/docs/inverse-dynamics.rst b/www/docs/inverse-dynamics.rst index eba2147f5..01bf1ce2e 100644 --- a/www/docs/inverse-dynamics.rst +++ b/www/docs/inverse-dynamics.rst @@ -24,11 +24,11 @@ Note that another name for :math:`\frac{v_{t+1} - v_t}{\Delta t}` is acceleratio In its simplest form, when dealing with a robot arm rigidly attached to a base on the ground, the above equation solves the problem. -This can be solved in code by calling :code:`Skeleton.getInverseDynamics(next_vel)`: +This can be solved in code by calling :code:`Skeleton.getInverseDynamics(accelerations)`: .. code-block:: python - control_forces = robot.getInverseDynamics(desired_next_vel) + control_forces = robot.getInverseDynamics(desired_acc) This equation doesn't respect joint force limits, because it's solving a linear system of equations which (if :math:`M` is full rank) has only one solution. @@ -51,7 +51,7 @@ In code, that looks like this: .. code-block:: python - result = robot.getContactInverseDynamics(desired_next_vel, right_foot) + result = robot.getContactInverseDynamics(desired_acc, right_foot) control_forces = result.jointTorques right_foot_contact_force = result.contactWrench @@ -101,7 +101,7 @@ To pick the nearest set of contact forces to some initial guess, expressed as a .. code-block:: python - result = robot.getMultipleContactInverseDynamics(next_vel, contact_bodies_list, body_wrench_guesses) + result = robot.getMultipleContactInverseDynamics(desired_acc, contact_bodies_list, body_wrench_guesses) control_forces = result.jointTorques first_foot_contact_force = result.contactWrench[0] second_foot_contact_force = result.contactWrench[1] @@ -110,7 +110,7 @@ To pick the set of contact forces that minimize the torques at each foot (on the .. code-block:: python - result = robot.getMultipleContactInverseDynamics(next_vel, contact_bodies_list) + result = robot.getMultipleContactInverseDynamics(desired_acc, contact_bodies_list) control_forces = result.jointTorques first_foot_contact_force = result.contactWrenches[0] second_foot_contact_force = result.contactWrenches[1]