In this course, we have developed models for serial chains, parallel mechanisms, floating-base systems, and constrained dynamics. We now use the term complex robot to denote systems that exhibit one or more of the following:

• High numbers of generalized coordinates \( n \) (e.g. humanoids, manipulators with many DOFs).
• Floating bases with multiple intermittent contacts (walking robots, quadrupeds).
• Closed kinematic loops and parallel structures with many constraints.
• Significant compliance, flexibility, or continuum behavior (cables, soft links, soft grippers).
• Strongly uncertain or time-varying parameters (payload, friction, contact properties).

A generic rigid-body dynamics model (including constraints and contact forces) can be written in the form

\[ \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) = \mathbf{S}^\top \boldsymbol{\tau} + \mathbf{J}_c(\mathbf{q})^\top \boldsymbol{\lambda} + \mathbf{d}(\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}}), \]

where \( \mathbf{d} \) lumps all effects not captured by the nominal rigid-body model (\( \mathbf{M},\mathbf{h},\mathbf{J}_c \)). For simple manipulators, we can usually make \( \mathbf{d} \approx \mathbf{0} \). For complex robots, \( \mathbf{d} \) is often large, structured, and hard to characterize, turning modeling into an open research problem.

flowchart TD
  A["Robot description (links, joints, contacts)"] --> B["Nominal rigid-body model M(q), h(q,dq)"]
  B --> C["Add constraints J_c(q), lambda"]
  C --> D["Augment with non-ideal effects: friction, compliance, delays"]
  D --> E["Identify / learn residual d(q,dq,ddq)"]
  E --> F["Use model in planning, estimation, control"]
  B -. "open: scalable algorithms, structure" .- B
  C -. "open: hybrid events, uniqueness" .- C
  D -. "open: soft bodies, continuum" .- D
  E -. "open: learning with guarantees" .- E
        

The rest of this lesson refines these high-level challenges into mathematical formulations that highlight why they remain open.

Earlier chapters introduced the rigid-body model \( \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}+ \mathbf{g}(\mathbf{q}) = \mathbf{S}^\top \boldsymbol{\tau} \), and its constrained variants. In practice we use a nominal model \( \hat{\mathbf{M}},\hat{\mathbf{C}},\hat{\mathbf{g}} \), derived from CAD, identification (Chapter 19), or prior data.

Let the true dynamics be

\[ \mathbf{M}_\star(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{h}_\star(\mathbf{q},\dot{\mathbf{q}}) = \mathbf{S}^\top \boldsymbol{\tau} + \mathbf{J}_c(\mathbf{q})^\top \boldsymbol{\lambda}, \]

and the nominal rigid-body model be

\[ \hat{\mathbf{M}}(\mathbf{q}) \ddot{\mathbf{q}} + \hat{\mathbf{h}}(\mathbf{q},\dot{\mathbf{q}}) = \mathbf{S}^\top \boldsymbol{\tau} + \hat{\mathbf{J}}_c(\mathbf{q})^\top \hat{\boldsymbol{\lambda}}. \]

The residual dynamics is then

\[ \mathbf{r}(\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}}) := \mathbf{M}_\star(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{h}_\star(\mathbf{q},\dot{\mathbf{q}}) - \Big( \hat{\mathbf{M}}(\mathbf{q}) \ddot{\mathbf{q}} + \hat{\mathbf{h}}(\mathbf{q},\dot{\mathbf{q}}) \Big). \]

We can rewrite the true dynamics as a nominal model plus residual:

\[ \hat{\mathbf{M}}(\mathbf{q}) \ddot{\mathbf{q}} + \hat{\mathbf{h}}(\mathbf{q},\dot{\mathbf{q}}) = \mathbf{S}^\top \boldsymbol{\tau} + \mathbf{J}_c(\mathbf{q})^\top \boldsymbol{\lambda} - \mathbf{r}(\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}}). \]

A common identification approach (Chapter 19) assumes that \( \mathbf{r} \) is linear in a parameter vector \( \boldsymbol{\theta} \):

\[ \mathbf{r}(\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}}) \approx \mathbf{Y}(\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}})\boldsymbol{\theta}, \]

where \( \mathbf{Y} \) is a regressor matrix. For complex robots, open problems include:

• Characterizing when \( \mathbf{Y} \) has sufficiently high rank to identify useful parameters.
• Imposing physical consistency (symmetry, positive definiteness of inertia) on the estimated residual.
• Ensuring that the combined model preserves properties like passivity and energy bounds.

One fundamental structural property of rigid-body dynamics is the skew-symmetry of \( \dot{\mathbf{M}}(\mathbf{q}) - 2\mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \):

\[ \dot{\mathbf{M}}(\mathbf{q}) - 2\mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) = -\Big(\dot{\mathbf{M}}(\mathbf{q}) - 2\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\Big)^\top. \]

Open question (structure-preserving learning): how to constrain data-driven residuals so that this skew-symmetry and positive-definiteness of \( \mathbf{M}(\mathbf{q}) \) are preserved?

Complex robots such as legged systems and manipulators in contact-rich environments exhibit hybrid dynamics: continuous flows interleaved with discrete impact and contact mode switches. A common modeling approach uses complementarity constraints for normal contacts:

\[ \phi(\mathbf{q}) \ge 0,\quad \lambda_n \ge 0,\quad \lambda_n \, \phi(\mathbf{q}) = 0, \]

where \( \phi(\mathbf{q}) \) is the signed distance and \( \lambda_n \) is the normal contact force. Together with Coulomb friction, the dynamics becomes a differential complementarity problem:

\[ \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) = \mathbf{S}^\top \boldsymbol{\tau} + \mathbf{J}_n(\mathbf{q})^\top \boldsymbol{\lambda}_n + \mathbf{J}_t(\mathbf{q})^\top \boldsymbol{\lambda}_t, \]

\[ \mathbf{0} = \mathbf{f}_\text{comp}\big(\phi(\mathbf{q}),\boldsymbol{\lambda}_n,\boldsymbol{\lambda}_t\big). \]

Even in low dimensions, these models may have multiple or no solutions for the accelerations and contact forces. Open problems include:

• Conditions for existence and uniqueness of solutions for multi-contact robots.
• Numerical schemes that are both stable and computationally efficient for high-DOF systems.
• Hybrid model formulations that are differentiable enough for gradient-based optimization (see Section 6).

In practice, simulators approximate complementarity using stiff penalty forces or regularization, which introduces additional modeling error into \( \mathbf{d} \) and complicates identification.

For floating-base robots with multiple contacts (Chapter 17), we often write the dynamics in partitioned form:

\[ \begin{bmatrix} \mathbf{M}_{\text{bb}} & \mathbf{M}_{\text{bj}} \\ \mathbf{M}_{\text{jb}} & \mathbf{M}_{\text{jj}} \end{bmatrix} \begin{bmatrix} \ddot{\mathbf{q}}_b \\ \ddot{\mathbf{q}}_j \end{bmatrix} + \begin{bmatrix} \mathbf{h}_b \\ \mathbf{h}_j \end{bmatrix} = \begin{bmatrix} \mathbf{0} \\ \boldsymbol{\tau} \end{bmatrix} + \mathbf{J}_c(\mathbf{q})^\top \boldsymbol{\lambda}, \]

where \( \mathbf{q}_b \) are floating-base coordinates and \( \mathbf{q}_j \) are joint coordinates. When \( n \) is large and many constraints are active, direct inversion becomes expensive and ill-conditioned.

For soft or continuum robots, configuration depends on a continuous material coordinate \( s \). A simplified one-dimensional continuum model is

\[ \rho(s) \frac{\partial^2 \mathbf{x}(s,t)}{\partial t^2} = \frac{\partial}{\partial s}\Big(\mathbf{T}(s,t)\Big) + \mathbf{f}(s,t), \]

where \( \rho(s) \) is mass density, \( \mathbf{T} \) internal stress, and \( \mathbf{f} \) distributed forces. To embed this into our generalized-coordinate framework, we approximate \( \mathbf{x}(s,t) \) using a finite set of shape functions \( \boldsymbol{\Phi}_i(s) \):

\[ \mathbf{x}(s,t) \approx \sum_{i=1}^r \boldsymbol{\Phi}_i(s) q_i(t), \]

leading to an approximate finite-dimensional model of the familiar Lagrangian form. Open questions include:

• Choosing basis functions that preserve passivity and stability.
• Balancing model order \( r \) with accuracy, especially under contact.
• Coupling continuum segments with rigid-body trees in a unified representation.

Building on probabilistic dynamics (Chapter 20, Lesson 3), assume the parameter vector \( \boldsymbol{\theta} \) (masses, inertias, friction) is random with mean \( \bar{\boldsymbol{\theta}} \) and covariance \( \boldsymbol{\Sigma}_\theta \). For a given configuration and command \( (\mathbf{q},\dot{\mathbf{q}},\boldsymbol{\tau}) \), the joint acceleration is a function \( \ddot{\mathbf{q}} = \mathbf{f}(\mathbf{q},\dot{\mathbf{q}},\boldsymbol{\tau};\boldsymbol{\theta}) \).

A first-order (linearized) uncertainty propagation gives

\[ \ddot{\mathbf{q}} \approx \mathbf{f}\big(\mathbf{q},\dot{\mathbf{q}},\boldsymbol{\tau};\bar{\boldsymbol{\theta}}\big) + \mathbf{J}_\theta(\mathbf{q},\dot{\mathbf{q}},\boldsymbol{\tau}) \big(\boldsymbol{\theta}-\bar{\boldsymbol{\theta}}\big), \]

where \( \mathbf{J}_\theta := \partial \mathbf{f} / \partial \boldsymbol{\theta} \). The covariance of the acceleration is then approximated as

\[ \boldsymbol{\Sigma}_{\ddot{q}} \approx \mathbf{J}_\theta \boldsymbol{\Sigma}_\theta \mathbf{J}_\theta^\top. \]

For complex robots with many uncertain contacts and parameters, \( \mathbf{J}_\theta \) is expensive to compute and the linear approximation may break down due to hybrid events. Open problems:

• Efficiently computing or approximating \( \mathbf{J}_\theta \) for high-DOF systems.
• Representing multi-modal uncertainty caused by contact mode switches.
• Constructing reduced-order probabilistic models that remain informative for planning and control.

Differentiable physics (Chapter 20, Lesson 4) seeks to compute gradients of trajectories with respect to parameters, initial conditions, or controls. For smooth ODEs \( \dot{\mathbf{x}} = \mathbf{f}(\mathbf{x},\boldsymbol{\theta}) \), sensitivities obey a linear variational equation. However, hybrid dynamics introduce discontinuities at impact times \( t_k \), where the state jumps via a reset map \( \mathbf{x}^+ = \boldsymbol{\Delta}(\mathbf{x}^-) \).

Ignoring contact, sensitivities satisfy

\[ \frac{\mathrm{d}}{\mathrm{d}t} \frac{\partial \mathbf{x}(t)}{\partial \boldsymbol{\theta}} = \frac{\partial \mathbf{f}}{\partial \mathbf{x}} \frac{\partial \mathbf{x}(t)}{\partial \boldsymbol{\theta}} + \frac{\partial \mathbf{f}}{\partial \boldsymbol{\theta}}. \]

At an impact time \( t_k \), the sensitivity jumps as

\[ \frac{\partial \mathbf{x}^+}{\partial \boldsymbol{\theta}} = \frac{\partial \boldsymbol{\Delta}}{\partial \mathbf{x}^-} \frac{\partial \mathbf{x}^-}{\partial \boldsymbol{\theta}} + \frac{\partial \boldsymbol{\Delta}}{\partial \boldsymbol{\theta}}. \]

For complementarity-based contacts, \( \boldsymbol{\Delta} \) is only piecewise defined and may not be differentiable. This leads to several open problems:

• Designing contact models that are smooth enough for differentiation, yet accurate.
• Handling event time sensitivities when impacts depend on parameters.
• Combining automatic differentiation with hybrid solvers at scale.

Many current approaches use smooth penalty methods for contact to obtain differentiable ODEs, but the relationship between the penalty limit and the nonsmooth complementarity model is still not fully understood.

We illustrate how a Python robotics stack can wrap a classical rigid-body dynamics library (e.g. pinocchio or a custom Chapter 12 implementation) with a residual term representing unmodeled effects. The residual can later be identified from data or approximated by a function approximator.

import numpy as np

class ComplexRobotModel:
    def __init__(self, pin_model, pin_data, residual_model=None):
        """
        pin_model, pin_data: nominal rigid-body model (e.g. from pinocchio).
        residual_model: callable r(q, dq, ddq) -> R^n or None.
        """
        self.pin_model = pin_model
        self.pin_data = pin_data
        self.residual_model = residual_model

    def inverse_dynamics_nominal(self, q, dq, ddq):
        """
        Wrapper around a standard recursive Newton-Euler algorithm.
        Any equivalent FK-based implementation from earlier chapters can be used.
        """
        import pinocchio as pin  # or your own implementation
        tau_nom = pin.rnea(self.pin_model, self.pin_data, q, dq, ddq)
        return np.array(tau_nom)

    def residual(self, q, dq, ddq):
        if self.residual_model is None:
            return np.zeros_like(q)
        return np.array(self.residual_model(q, dq, ddq))

    def inverse_dynamics(self, q, dq, ddq):
        """
        tau = tau_nominal - r(q, dq, ddq)
        """
        tau_nom = self.inverse_dynamics_nominal(q, dq, ddq)
        r_val = self.residual(q, dq, ddq)
        return tau_nom - r_val

# Example: linear-in-parameters residual model
class LinearResidual:
    def __init__(self, theta):
        self.theta = np.asarray(theta)

    def __call__(self, q, dq, ddq):
        # Very simple regressor using stacked state
        phi = np.concatenate([q, dq, ddq, np.ones_like(q)])
        # Build block-diagonal regressor for each joint
        n = q.shape[0]
        # Phi_full has shape (n, len(phi))
        Phi_full = np.tile(phi, (n, 1))
        # residual r = Phi_full @ theta, reshaped
        return Phi_full @ self.theta

# Usage (assuming pin_model, pin_data are available)
# residual_model = LinearResidual(theta=np.zeros(3*n + 1))
# robot = ComplexRobotModel(pin_model, pin_data, residual_model)
# tau = robot.inverse_dynamics(q, dq, ddq)
      

Open issues illustrated by this code:

• How to choose a regressor that captures meaningful physics but remains low-dimensional?
• How to constrain residual_model so that energy and passivity properties of the nominal model are preserved?
• How to incorporate contact forces and hybrid events consistently into the residual term?

In C++, widely used libraries such as RBDL or Pinocchio provide efficient Newton–Euler and articulated-body algorithms. We sketch a plugin-style interface that allows injecting residual models while keeping the base dynamics library unchanged.

#include <rbdl/rbdl.h>
#include <vector>

struct ResidualModel {
  virtual ~ResidualModel() {}
  virtual void compute(const RigidBodyDynamics::Math::VectorNd& q,
                       const RigidBodyDynamics::Math::VectorNd& dq,
                       const RigidBodyDynamics::Math::VectorNd& ddq,
                       RigidBodyDynamics::Math::VectorNd& r_out) const = 0;
};

struct ZeroResidual : public ResidualModel {
  void compute(const RigidBodyDynamics::Math::VectorNd& q,
               const RigidBodyDynamics::Math::VectorNd& dq,
               const RigidBodyDynamics::Math::VectorNd& ddq,
               RigidBodyDynamics::Math::VectorNd& r_out) const override {
    r_out.setZero();
  }
};

struct ComplexRobotModelCpp {
  RigidBodyDynamics::Model model;
  ResidualModel* residual;

  ComplexRobotModelCpp(const RigidBodyDynamics::Model& m,
                       ResidualModel* residual_model)
    : model(m), residual(residual_model) {}

  void inverse_dynamics(const RigidBodyDynamics::Math::VectorNd& q,
                        const RigidBodyDynamics::Math::VectorNd& dq,
                        const RigidBodyDynamics::Math::VectorNd& ddq,
                        RigidBodyDynamics::Math::VectorNd& tau_out) {
    using namespace RigidBodyDynamics;
    using namespace RigidBodyDynamics::Math;

    VectorNd tau_nom = VectorNd::Zero(model.qdot_size);
    RigidBodyDynamics::InverseDynamics(model, q, dq, ddq, tau_nom);

    VectorNd r = VectorNd::Zero(model.qdot_size);
    if (residual) {
      residual->compute(q, dq, ddq, r);
    }
    tau_out = tau_nom - r;
  }
};
      

This pattern separates nominal dynamics from residual modeling, but does not yet address:

• Efficient treatment of many contact modes in InverseDynamics.
• Real-time updating of parameters and residuals from streaming data.
• Preservation of structural properties (symmetry, sparsity) in modified inertia matrices.

While Java has fewer robotics-specific libraries, one can implement modeling abstractions on top of linear algebra libraries (e.g. EJML). Below is a simplified Java skeleton for a graph-based robot model, mapping configuration, velocity, and joint torques to accelerations via a user-defined dynamics routine.

import org.ejml.simple.SimpleMatrix;
import java.util.List;

interface DynamicsFunctor {
    SimpleMatrix forwardDynamics(SimpleMatrix q,
                                 SimpleMatrix dq,
                                 SimpleMatrix tau);
}

class Link {
    public String name;
    public int index;
    public Link(String name, int index) {
        this.name = name;
        this.index = index;
    }
}

class Joint {
    public String name;
    public int parentIndex;
    public int childIndex;
    public int dof;
    public Joint(String name, int parentIndex, int childIndex, int dof) {
        this.name = name;
        this.parentIndex = parentIndex;
        this.childIndex = childIndex;
        this.dof = dof;
    }
}

class RobotModelJava {
    private List<Link> links;
    private List<Joint> joints;
    private DynamicsFunctor dynamics;

    public RobotModelJava(List<Link> links,
                          List<Joint> joints,
                          DynamicsFunctor dynamics) {
        this.links = links;
        this.joints = joints;
        this.dynamics = dynamics;
    }

    public SimpleMatrix forwardDynamics(SimpleMatrix q,
                                        SimpleMatrix dq,
                                        SimpleMatrix tau) {
        return dynamics.forwardDynamics(q, dq, tau);
    }
}
      

Here, the DynamicsFunctor can encode classical rigid-body dynamics, residual models, or even hybrid approximations. Open problems remain in:

• Representing contact modes and constraints in a graph-based API.
• Maintaining numerical stability for large articulated graphs.
• Integrating such models with simulation engines and verification tools.

MATLAB and Simulink (with Robotics System Toolbox and Simscape Multibody) are widely used for modeling complex robots. The following script illustrates how one might set up uncertain parameters for a manipulator model and evaluate their effect on joint torques.

% Nominal parameters
n = 6;                    % DOF
m_nom = 5 * ones(n,1);    % nominal link masses

% Uncertain masses (interval +-20 percent)
m_unc = ureal('m_unc', m_nom(1), 'Percentage', 20);
% For simplicity, treat all links as having the same uncertain mass here.

% Example state
q  = zeros(n,1);
dq = zeros(n,1);
ddq = ones(n,1);

% Placeholder nominal inverse dynamics (to be replaced by your Lagrange / Newton-Euler code)
function tau = invdyn_nominal(q, dq, ddq, m_val)
    n = numel(q);
    M = eye(n) * m_val;    % crude mass matrix
    h = zeros(n,1);
    tau = M * ddq + h;
end

tau_samples = [];
for k = 1:100
    % Sample an uncertain mass value
    m_sample = usample(m_unc);
    tau_k = invdyn_nominal(q, dq, ddq, m_sample);
    tau_samples = [tau_samples, tau_k];
end

% Analyze spread of torques due to uncertainty
tau_mean = mean(tau_samples, 2);
tau_std  = std(tau_samples, 0, 2);
disp(tau_mean);
disp(tau_std);
      

In Simulink, the same idea can be realized by:

• Building a multi-body model with Simscape Multibody blocks.
• Replacing fixed inertial parameters with uncertain blocks or tunable parameters.
• Running Monte Carlo simulations via the Simulink Design Optimization interface.

Open questions: how to systematically reduce such uncertain multi-body models while preserving worst-case bounds on trajectories and forces?

Mathematica is well suited for symbolic derivations of equations of motion and for adding symbolic residual terms. Below is a sketch for a 2-DOF planar arm whose Lagrangian we derived in earlier chapters. We add abstract residual torques \( d_1(t), d_2(t) \) and export the result as a numeric function.

Clear[q1, q2, dq1, dq2, t];
q1 = q1[t]; q2 = q2[t];
dq1 = D[q1, t]; dq2 = D[q2, t];

(* Parameters *)
m1 = Symbol["m1"]; m2 = Symbol["m2"];
l1 = Symbol["l1"]; l2 = Symbol["l2"];
g  = Symbol["g"];

(* Kinetic and potential energy for a simple planar 2R arm *)
T = 1/2 m1 (l1^2 dq1^2) +
    1/2 m2 ((l1^2 dq1^2) + (l2^2 (dq1 + dq2)^2) +
            2 l1 l2 dq1 (dq1 + dq2) Cos[q2]);
V = m1 g (l1/2) Cos[q1] +
    m2 g (l1 Cos[q1] + l2/2 Cos[q1 + q2]);

L = T - V;

tau1 = Symbol["tau1"][t];
tau2 = Symbol["tau2"][t];
d1   = Symbol["d1"][t];   (* residual torque joint 1 *)
d2   = Symbol["d2"][t];

eom1 = D[D[L, dq1], t] - D[L, q1] == tau1 - d1;
eom2 = D[D[L, dq2], t] - D[L, q2] == tau2 - d2;

(* Solve symbolically for accelerations (may be expensive) *)
sol = Solve[{eom1, eom2}, {D[dq1, t], D[dq2, t]}];

(* Generate a numeric function for use in simulation *)
fdyn = Function[{q1val, q2val, dq1val, dq2val, tau1val, tau2val, d1val, d2val,
                 m1val, m2val, l1val, l2val, gval},
    Module[{res},
      res = {D[dq1, t], D[dq2, t]} /. sol[[1]];
      res /. {q1 -> q1val, q2 -> q2val,
              dq1 -> dq1val, dq2 -> dq2val,
              tau1 -> tau1val, tau2 -> tau2val,
              d1 -> d1val, d2 -> d2val,
              m1 -> m1val, m2 -> m2val,
              l1 -> l1val, l2 -> l2val, g -> gval}
    ]
];
      

This framework allows one to treat the residual torques \( d_1,d_2 \) as symbolic inputs (for example, from an identification or learning algorithm) and study their effect on stability and energy.

This lesson extended the classical modeling framework developed throughout the course to complex robots: high-DOF, floating-base, contact-rich, and compliant systems with uncertainty. We formalized model error as residual dynamics, highlighted the difficulties of hybrid contact models, examined continuum approximations, and discussed probabilistic and differentiable physics viewpoints. Code sketches in Python, C++, Java, MATLAB/Simulink, and Mathematica illustrated how current software encapsulates these ideas while exposing open theoretical questions: structure-preserving learning, scalable uncertainty propagation, and hybrid sensitivity analysis. These open problems are central to current research in robotics modeling.