This appendix provides the formal mathematical definitions and physical derivations for the continuous equations of motion governing the UR5e manipulator and its contact interactions in MuJoCo.

---

A.1 The Continuous Equations of Motion

For an $n$-degree-of-freedom serial robotic manipulator with generalized coordinates $q \in \mathbb{R}^n$, the multi-body equations of motion derived from the Euler-Lagrange equations:

$$\frac{d}{dt}\left(\frac{\partial \mathcal{L}}{\partial \dot{q}}\right) - \frac{\partial \mathcal{L}}{\partial q} = \tau_{\text{gen}}$$

In Plain English (No Math Jargon)

Whenever a robotic arm moves, its motors have to fight against two things:

1. The effort to speed up heavy metal (Inertia): A heavy arm resists starting to move, and resists stopping once it is in motion.
2. The downward pull of gravity: Heavy metal links are constantly being dragged down toward the floor.

The equation simply states:

The push from the motor = The force needed to accelerate the arm + The force needed to hold it up against gravity.

Why does this equation exist?

A robot arm has six hinges stacked on top of each other. When the shoulder turns, the elbow swings, the wrist twists, and the forces interact in a tangled mess.

Trying to calculate those forces joint-by-joint with geometric vector diagrams would require hundreds of tedious, error-prone calculations.

This equation is a master shortcut: you simply write down the total energy of the robot (its kinetic energy of motion and its potential energy of height), and the formula automatically tells you:

"To make this movement happen, here is exactly how hard each motor needs to push."

---

Yielding the canonical manipulator dynamic formulation:

$$\mathbf{M}(q)\ddot{q} + \mathbf{C}(q, \dot{q})\dot{q} + \mathbf{g}(q) = \tau + \mathbf{J}(q)^T f_{\text{ext}}$$

---

A.2 Detailed Breakdown of Terms

1. The Mass Matrix: $\mathbf{M}(q)\ddot{q}$

$\mathbf{M}(q) \in \mathbb{R}^{n \times n}$ is the configuration-dependent mass matrix (or inertia matrix), and $\ddot{q} \in \mathbb{R}^n$ is the vector of joint angular accelerations.

• Properties: $\mathbf{M}(q)$ is symmetric, strictly positive-definite ($\mathbf{M}(q) = \mathbf{M}(q)^T > 0$), and bounded for all finite link dimensions.
• Pose Dependence: The kinetic energy of the manipulator is $\mathcal{K} = \frac{1}{2}\dot{q}^T \mathbf{M}(q)\dot{q}$. When the arm is outstretched horizontally, its rotational inertia about the base and shoulder is maximal; when folded inward, the rotational inertia is minimized.
• Dynamic Cross-Coupling: The off-diagonal entries $M_{ij}(q)$ ($i \neq j$) represent dynamic coupling between joints. Accelerating joint $j$ creates an immediate inertial torque on joint $i$.

2. Coriolis and Centrifugal Torques: $\mathbf{C}(q, \dot{q})\dot{q}$

$\mathbf{C}(q, \dot{q}) \in \mathbb{R}^{n \times n}$ accounts for velocity-dependent fictitious inertial forces arising from the rotating frames of reference:

• Centrifugal terms: Proportional to $\dot{q}_i^2$, pulling rotating links radially outward.
• Coriolis terms: Proportional to $\dot{q}_i \dot{q}_j$ ($i \neq j$), arising from mutual relative motion of links in rotating frames.
• Christoffel Symbols: The elements of $\mathbf{C}$ are uniquely defined via Christoffel symbols of the first kind: $$C_{kj}(q, \dot{q}) = \sum_{i=1}^n c_{ijk}(q)\dot{q}_i = \frac{1}{2}\sum_{i=1}^n \left( \frac{\partial M_{kj}}{\partial q_i} + \frac{\partial M_{ki}}{\partial q_j} - \frac{\partial M_{ij}}{\partial q_k} \right) \dot{q}_i$$
• Skew-Symmetry: The matrix $\dot{\mathbf{M}}(q) - 2\mathbf{C}(q, \dot{q})$ is skew-symmetric, ensuring conservation of mechanical energy: $$x^T \Big(\dot{\mathbf{M}}(q) - 2\mathbf{C}(q, \dot{q})\Big) x = 0, \quad \forall x \in \mathbb{R}^n$$

3. Gravitational Torques: $\mathbf{g}(q)$

The gravity vector $\mathbf{g}(q) \in \mathbb{R}^n$ represents the static torque required to hold the manipulator in configuration $q$ against the gravitational potential energy $\mathcal{P}(q)$:

$$g_i(q) = \frac{\partial \mathcal{P}(q)}{\partial q_i} = -\sum_{j=1}^n m_j \mathbf{g}_0^T \mathbf{J}_{v,j}^{(i)}(q)$$

where $m_j$ is the mass of link $j$, $\mathbf{g}_0 = [0, 0, -9.81]^T$ is the gravity vector, and $\mathbf{J}_{v,j}$ is the linear Jacobian of link $j$'s center of mass.

4. Actuator Torques: $\tau$

$\tau \in \mathbb{R}^n$ is the vector of generalized torques delivered by the joint actuators. In our setup, these are synthesized by proportional-derivative (PD) servos driven toward the middle-level setpoints $q^*$:

$$\tau = \text{clip}\Big(\mathbf{K}_p(q^* - q) - \mathbf{K}_d \dot{q}, -\tau_{\max}, \tau_{\max}\Big)$$

5. Contact Wrench Mapping via Jacobian Transpose: $\mathbf{J}(q)^T f_{\text{ext}}$

When the end-effector contacts an object or surface, an external wrench $f_{\text{ext}} = [F_x, F_y, F_z, M_x, M_y, M_z]^T \in \mathbb{R}^6$ acts on the tool center point. By the principle of virtual work:

$$\delta W = f_{\text{ext}}^T \delta x = f_{\text{ext}}^T (\mathbf{J}(q) \delta q) = (\mathbf{J}(q)^T f_{\text{ext}})^T \delta q = \tau_{\text{ext}}^T \delta q$$

Consequently, the equivalent joint reaction torque is:

$$\tau_{\text{ext}} = \mathbf{J}(q)^T f_{\text{ext}}$$

---

A.3 Contact Dynamics and Complementarity in MuJoCo

In unconstrained free space ($f_{\text{ext}} = \mathbf{0}$), joint accelerations are computed simply by inverting $\mathbf{M}(q)$:

$$\ddot{q} = \mathbf{M}(q)^{-1} \Big( \tau - \mathbf{C}(q, \dot{q})\dot{q} - \mathbf{g}(q) \Big)$$

When objects contact each other, the simulator must enforce hard non-penetration and frictional constraints:

1. Signorini Complementarity Conditions (Non-Penetration):

   $$d \ge 0, \quad f_N \ge 0, \quad d \cdot f_N = 0$$

   where $d$ is signed distance between surfaces and $f_N$ is normal contact force.

2. Coulomb Friction Cone Constraints:

   $$\|f_T\| \le \mu f_N$$

   where $f_T$ is tangential frictional force and $\mu$ is the friction coefficient.

MuJoCo formulates this continuous multi-body contact problem as a convex Quadratic Program (QP) / Linear Complementarity Problem (LCP), solved at $500\text{ Hz}$ ($2\text{ ms}$ timestep) to generate physically stable, chatter-free contact forces.