Consider an end-effector interacting with a smooth surface, such as a robot hand sliding along a wall while pushing with a prescribed normal force. Intuitively, we wish to:

• Control position/velocity tangentially to the surface (motion directions where the environment does not constrain the robot).
• Control force in the direction normal to the surface (where rigid contact prevents motion but allows a contact wrench).

Let \( \mathbf{q}\in\mathbb{R}^n \) be joint coordinates and \( \mathbf{x}\in\mathbb{R}^m \) a task-space coordinate (e.g. end-effector pose parameters). We assume students are familiar with:

• Forward kinematics \( \mathbf{x} = \mathbf{f}(\mathbf{q}) \).
• Geometric Jacobian \( \mathbf{J}(\mathbf{q}) = \frac{\partial \mathbf{f}}{\partial \mathbf{q}} \).
• Manipulator dynamics in joint space and task space (from the prior robotics course).

Hybrid position/force control introduces a task decomposition by means of selection matrices. In a local contact frame, we separate motion/force components parallel and orthogonal to the contact manifold. Let \( \mathbf{n}\in\mathbb{R}^3 \) be the unit normal at the contact point (in end-effector space).

Conceptually, each control cycle performs:

flowchart TD
  Q["Joint state q, qdot"] --> KIN["Forward kinematics / Jacobian"]
  KIN --> X["Task state x, xdot"]
  ENV["Contact frame and unit normal n"] --> SEL["Build selection matrices S_pos, S_force"]
  X --> ERR["Compute position and force errors"]
  SEL --> ERR
  ERR --> LAW["Hybrid law: position control in tangent, force control in normal"]
  LAW --> WRENCH["Task wrench F_cmd"]
  WRENCH --> TORQUE["Joint torques tau = J^T F_cmd + model terms"]
  TORQUE --> ROB["Low-level torque servo"]
        

The remainder of this lesson formalizes each block mathematically, proves key decoupling properties under ideal conditions, and provides implementation patterns.

We recall the standard rigid manipulator dynamics with an external wrench \( \mathbf{F}_c \) applied at the end-effector:

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau} + \mathbf{J}(\mathbf{q})^\top \mathbf{F}_c . \]

Here \( \mathbf{M} \) is the inertia matrix, \( \mathbf{C}\dot{\mathbf{q}} \) the Coriolis/centrifugal terms, and \( \mathbf{g} \) the gravity vector. The task-space coordinates satisfy \( \dot{\mathbf{x}} = \mathbf{J}(\mathbf{q})\dot{\mathbf{q}} \).

Under standard assumptions (nonsingular Jacobian, full row rank), an operational-space dynamics can be derived:

\[ \boldsymbol{\Lambda}(\mathbf{q}) \ddot{\mathbf{x}} + \boldsymbol{\mu}(\mathbf{q},\dot{\mathbf{q}}) + \mathbf{p}(\mathbf{q}) = \mathbf{F}_u + \mathbf{F}_c, \]

where the task-space inertia is given by \( \boldsymbol{\Lambda}(\mathbf{q}) = \bigl(\mathbf{J}(\mathbf{q})\mathbf{M}(\mathbf{q})^{-1}\mathbf{J}(\mathbf{q})^\top\bigr)^{-1} \), \( \boldsymbol{\mu} \) collects centrifugal/Coriolis projections, and \( \mathbf{p} \) is the projected gravity term. The vector \( \mathbf{F}_u \) is the control wrench generated by the joints:

\[ \boldsymbol{\tau} = \mathbf{J}(\mathbf{q})^\top \mathbf{F}_u. \]

The environment is described locally by a smooth scalar function \( \phi(\mathbf{x}) \) such that \( \phi(\mathbf{x}) = 0 \) on the contact manifold. The (non-normalized) surface normal is \( \nabla\phi(\mathbf{x}) \); the unit normal is

\[ \mathbf{n}(\mathbf{x}) = \frac{\nabla\phi(\mathbf{x})}{\|\nabla\phi(\mathbf{x})\|}. \]

Under rigid unilateral contact (no penetration, no adhesion), the normal distance is constrained:

\[ d_n(\mathbf{x}) \doteq \phi(\mathbf{x}) \approx 0, \quad d_n(\mathbf{x}) \ge 0, \quad f_n \ge 0, \quad d_n(\mathbf{x}) f_n = 0, \]

where \( f_n \) is the normal component of the contact force. The tangent components \( \mathbf{F}_t \) may be associated with friction; in this lesson, we focus on the normal reaction and assume sticking contact in tangential directions.

We decompose the contact wrench as

\[ \mathbf{F}_c = \mathbf{F}_t + f_n \mathbf{n}, \]

and enforce the no-penetration constraint by suitable control design together with physical contact. Hybrid position/force control explicitly exploits this decomposition at the controller level.

Let \( \mathbf{x}\in\mathbb{R}^m \) denote the task-space coordinate vector. Hybrid control introduces selection matrices:

\[ \mathbf{S}_p \in \mathbb{R}^{m\times m}, \quad \mathbf{S}_f \in \mathbb{R}^{m\times m}, \]

selecting, respectively, components for position (or motion) control and force control. They satisfy

\[ \mathbf{S}_p^2 = \mathbf{S}_p,\quad \mathbf{S}_f^2 = \mathbf{S}_f,\quad \mathbf{S}_p \mathbf{S}_f = \mathbf{0},\quad \mathbf{S}_p + \mathbf{S}_f = \mathbf{I}. \]

Thus \( \mathbf{S}_p \) and \( \mathbf{S}_f \) are complementary projectors. In a local contact frame where the first axis is aligned with the normal \( \mathbf{n} \), a common choice for a 3D translation-only interface is

\[ \mathbf{S}_f = \mathbf{n}\mathbf{n}^\top,\quad \mathbf{S}_p = \mathbf{I} - \mathbf{n}\mathbf{n}^\top. \]

The position-controlled subspace is the tangent space to the surface; the force-controlled subspace is the normal direction.

Let \( \mathbf{x}_d(t) \) be a desired end-effector trajectory and \( \mathbf{F}_d(t) \) a desired contact force profile. We decompose the objectives as

\[ \mathbf{x}_p = \mathbf{S}_p \mathbf{x},\quad \mathbf{x}_{p,d} = \mathbf{S}_p \mathbf{x}_d,\quad \mathbf{F}_f = \mathbf{S}_f \mathbf{F}_c,\quad \mathbf{F}_{f,d} = \mathbf{S}_f \mathbf{F}_d. \]

Define position and force errors:

\[ \mathbf{e}_p = \mathbf{x}_{p,d} - \mathbf{x}_p,\quad \dot{\mathbf{e}}_p = \dot{\mathbf{x}}_{p,d} - \dot{\mathbf{x}}_p,\quad \mathbf{e}_f = \mathbf{F}_{f,d} - \mathbf{F}_f. \]

A basic hybrid law in task space can be written as

\[ \mathbf{F}_u = \underbrace{\mathbf{S}_p \bigl( \mathbf{K}_p \mathbf{e}_p + \mathbf{K}_d \dot{\mathbf{e}}_p \bigr)}_{\text{position loop in tangent subspace}} \;+\; \underbrace{\mathbf{S}_f \bigl( \mathbf{F}_{f,d} + \mathbf{K}_f \mathbf{e}_f \bigr)}_{\text{force loop in normal subspace}}, \]

where \( \mathbf{K}_p, \mathbf{K}_d \) are positive-definite gain matrices on the position-controlled subspace, and \( \mathbf{K}_f \) is a positive-definite gain matrix (often diagonal) in the force-controlled subspace. The model-based version adds operational-space dynamics compensation:

\[ \mathbf{F}_u = \boldsymbol{\Lambda}(\mathbf{q}) \mathbf{a}_d + \boldsymbol{\mu}(\mathbf{q},\dot{\mathbf{q}}) + \mathbf{p}(\mathbf{q}), \]

where the desired acceleration vector is

\[ \mathbf{a}_d = \mathbf{S}_p \bigl( \ddot{\mathbf{x}}_{p,d} + \mathbf{K}_p \mathbf{e}_p + \mathbf{K}_d \dot{\mathbf{e}}_p \bigr) + \mathbf{S}_f \mathbf{a}_f. \]

The term \( \mathbf{a}_f \) is chosen so that the resulting normal acceleration and the contact model produce the desired force (for rigid contact, one often uses a pure force feedback as above rather than an explicit acceleration).

3.1 Decoupling Argument (Idealized)

Under the assumptions:

• Rigid contact restricts motion along the normal direction, so the true acceleration satisfies \( \mathbf{S}_f \ddot{\mathbf{x}} \approx \mathbf{0} \).
• The contact force acts only in the normal direction, \( \mathbf{F}_c = \mathbf{S}_f \mathbf{F}_c \), so \( \mathbf{S}_p \mathbf{F}_c = \mathbf{0} \).
• The projection matrices commute with \( \boldsymbol{\Lambda} \) in the sense that cross-coupling terms are small (this holds exactly if the inertia is block-diagonal in the chosen contact frame).

Then the tangent component of the dynamics:

\[ \mathbf{S}_p \boldsymbol{\Lambda} \ddot{\mathbf{x}} + \mathbf{S}_p (\boldsymbol{\mu} + \mathbf{p}) = \mathbf{S}_p \mathbf{F}_u + \underbrace{\mathbf{S}_p \mathbf{F}_c}_{=\,\mathbf{0}} \]

is driven only by the position-control term \( \mathbf{S}_p (\mathbf{K}_p \mathbf{e}_p + \mathbf{K}_d \dot{\mathbf{e}}_p) \), whereas the normal direction is governed predominantly by the force-control term. Because \( \mathbf{S}_p \mathbf{S}_f = \mathbf{0} \), the two loops decouple in the ideal case, which is the central design objective of hybrid control.

Once the hybrid law provides the task wrench \( \mathbf{F}_u \), the corresponding joint torques are

\[ \boldsymbol{\tau} = \mathbf{J}(\mathbf{q})^\top \mathbf{F}_u + \boldsymbol{\tau}_{\text{bias}}, \]

where \( \boldsymbol{\tau}_{\text{bias}} \) can compensate for gravity and Coriolis terms using standard inverse-dynamics:

\[ \boldsymbol{\tau}_{\text{bias}} = \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}). \]

For redundant manipulators (\( n > m \)), it is common to add a null-space term:

\[ \boldsymbol{\tau} = \mathbf{J}^\top \mathbf{F}_u + \mathbf{N}^\top \boldsymbol{\tau}_0, \quad \mathbf{N} = \mathbf{I}_n - \mathbf{J}^\top(\mathbf{J}\mathbf{J}^\top)^{-1}\mathbf{J}, \]

where \( \boldsymbol{\tau}_0 \) can be used to control secondary tasks (posture, joint limits) without affecting the contact wrench, as discussed in task-priority control (Chapter 4).

4.1 Example: 3D Point, Planar Surface

Suppose the end-effector position \( \mathbf{x}\in\mathbb{R}^3 \) is constrained to move on a plane with unit normal \( \mathbf{n} \). Then the selection matrices are

\[ \mathbf{S}_f = \mathbf{n}\mathbf{n}^\top,\quad \mathbf{S}_p = \mathbf{I}_3 - \mathbf{n}\mathbf{n}^\top. \]

Let \( F_{n,d} \) be the desired normal force and \( \mathbf{x}_d \) a desired tangent trajectory (consistent with the plane). Using a scalar gain \( k_f > 0 \) and tangent gains \( \mathbf{K}_p, \mathbf{K}_d \in\mathbb{R}^{3\times 3} \), one can implement

\[ \mathbf{F}_u = \mathbf{S}_p\bigl(\mathbf{K}_p (\mathbf{x}_d - \mathbf{x}) + \mathbf{K}_d (\dot{\mathbf{x}}_d - \dot{\mathbf{x}})\bigr) + \underbrace{(F_{n,d} + k_f(F_{n,d}-F_n))\mathbf{n}}_{\text{normal force control}}, \]

where \( F_n = \mathbf{n}^\top \mathbf{F}_c \) is the measured normal force (from a force/torque sensor). This expression is directly implementable and forms the core of the next section's code examples.

A practical hybrid position/force controller for a contact task with known surface normal follows a fixed loop at each control period. The high-level flow is:

flowchart TD
  S0["Start control cycle"] --> S1["Read q, qdot, force sensor"]
  S1 --> S2["Compute x, xdot, J from model"]
  S2 --> S3["Determine unit normal n and build S_pos, S_force"]
  S3 --> S4["Project desired x_d onto tangent plane"]
  S4 --> S5["Compute position error e_p and derivative"]
  S1 --> S6["Extract measured normal force F_n"]
  S5 --> S7["Compute tangential wrench F_t_cmd (PD)"]
  S6 --> S8["Compute normal wrench F_n_cmd = \nF_nd + k_f*(F_nd - F_n)"]
  S7 --> S9["Combine F_cmd = F_t_cmd + F_n_cmd * n"]
  S8 --> S9
  S9 --> S10["Map to joint torques tau = J^T F_cmd + bias"]
  S10 --> S11["Send tau to low-level controller"]
        

The code examples below are concrete realizations of this flow in several programming environments.

We consider a simple Python implementation using numpy and a generic robot model interface inspired by libraries such as pinocchio or roboticstoolbox-python. We assume:

• robot.forward_kinematics(q) returns the end-effector position x.
• robot.jacobian(q) returns the geometric Jacobian at the end-effector.
• robot.inverse_dynamics(q, qdot, qddot) returns the feedforward torque \( \mathbf{M}\ddot{\mathbf{q}} + \mathbf{C}\dot{\mathbf{q}} + \mathbf{g} \).
• robot.force_sensor() returns the measured end-effector force vector in the same frame as x.

import numpy as np

class HybridPositionForceController:
    """
    Hybrid position/force controller for a 3D point interacting with a planar surface.
    Position is controlled in the tangent plane; force is controlled along the normal.
    """

    def __init__(self, robot, contact_normal_world,
                 Kp_tan=None, Kd_tan=None, kf=50.0):
        self.robot = robot
        n = np.asarray(contact_normal_world, dtype=float).reshape(3)
        self.n = n / np.linalg.norm(n)
        self.S_f = np.outer(self.n, self.n)          # force-controlled projector
        self.S_p = np.eye(3) - self.S_f              # position-controlled projector

        if Kp_tan is None:
            # Default gains: only tangent directions matter (S_p will zero out normal)
            self.Kp_tan = 200.0 * np.eye(3)
        else:
            self.Kp_tan = np.asarray(Kp_tan, dtype=float)

        if Kd_tan is None:
            self.Kd_tan = 40.0 * np.eye(3)
        else:
            self.Kd_tan = np.asarray(Kd_tan, dtype=float)

        self.kf = float(kf)

    def compute_torque(self, q, qdot, x_d, xdot_d, Fd_n):
        """
        q, qdot: joint position/velocity (shape: (n,))
        x_d, xdot_d: desired position/velocity in R^3 (end-effector)
        Fd_n: desired normal force (scalar, positive when pushing along +n)
        """
        q = np.asarray(q, dtype=float).ravel()
        qdot = np.asarray(qdot, dtype=float).ravel()
        x_d = np.asarray(x_d, dtype=float).reshape(3)
        xdot_d = np.asarray(xdot_d, dtype=float).reshape(3)

        # Forward kinematics and Jacobian
        x = self.robot.forward_kinematics(q)         # shape (3,)
        J = self.robot.jacobian(q)                   # shape (3, n)
        xdot = J @ qdot

        # Project desired position onto tangent plane (avoid inconsistency)
        # x_d_proj = x_d - (n^T (x_d - x)) * n
        delta = x_d - x
        normal_component = (self.n @ delta) * self.n
        x_d_proj = x_d - normal_component

        # Position error (tangent only)
        e_p = self.S_p @ (x_d_proj - x)
        edot_p = self.S_p @ (xdot_d - xdot)

        # Tangential wrench command
        F_t_cmd = self.S_p @ (self.Kp_tan @ e_p + self.Kd_tan @ edot_p)

        # Measured force and normal component
        F_meas = self.robot.force_sensor().reshape(3)
        F_n = float(self.n @ F_meas)
        e_f = Fd_n - F_n

        # Normal wrench command (scalar times normal direction)
        F_n_cmd = Fd_n + self.kf * e_f
        F_n_vec = F_n_cmd * self.n

        # Total command wrench
        F_cmd = F_t_cmd + F_n_vec

        # Map to joint torques and add bias (inverse dynamics with zero qddot)
        tau = J.T @ F_cmd
        tau_bias = self.robot.inverse_dynamics(q, qdot, np.zeros_like(q))
        tau_total = tau + tau_bias
        return tau_total
      

The controller uses explicit tangent/normal projectors and a separate position loop (tangent) and force loop (normal). For a more accurate operational-space implementation, one could compute \( \boldsymbol{\Lambda} \), \( \boldsymbol{\mu} \), and \( \mathbf{p} \) and replace the inverse-dynamics call by operational-space inverse dynamics.

The following C++ snippet illustrates the same controller structure using Eigen. We assume a model interface providing methods forwardKinematics, jacobian, inverseDynamics, and forceSensor.

#include <Eigen/Dense>

class RobotModel {
public:
    // User-provided implementations
    Eigen::Vector3d forwardKinematics(const Eigen::VectorXd &q) const;
    Eigen::Matrix<double, 3, Eigen::Dynamic> jacobian(const Eigen::VectorXd &q) const;
    Eigen::VectorXd inverseDynamics(const Eigen::VectorXd &q,
                                    const Eigen::VectorXd &qdot,
                                    const Eigen::VectorXd &qddot) const;
    Eigen::Vector3d forceSensor() const;
};

class HybridPositionForceController {
public:
    HybridPositionForceController(const RobotModel *robot,
                                  const Eigen::Vector3d &contactNormalWorld)
        : robot_(robot), kf_(50.0)
    {
        n_ = contactNormalWorld.normalized();
        S_f_ = n_ * n_.transpose();
        S_p_ = Eigen::Matrix3d::Identity() - S_f_;

        Kp_tan_ = 200.0 * Eigen::Matrix3d::Identity();
        Kd_tan_ = 40.0 * Eigen::Matrix3d::Identity();
    }

    Eigen::VectorXd computeTorque(const Eigen::VectorXd &q,
                                  const Eigen::VectorXd &qdot,
                                  const Eigen::Vector3d &x_d,
                                  const Eigen::Vector3d &xdot_d,
                                  double Fd_n)
    {
        // Forward kinematics and Jacobian
        Eigen::Vector3d x = robot_->forwardKinematics(q);
        Eigen::Matrix<double, 3, Eigen::Dynamic> J = robot_->jacobian(q);
        Eigen::Vector3d xdot = J * qdot;

        // Project desired position onto tangent plane
        Eigen::Vector3d delta = x_d - x;
        Eigen::Vector3d normal_component = (n_.dot(delta)) * n_;
        Eigen::Vector3d x_d_proj = x_d - normal_component;

        // Position errors in tangent space
        Eigen::Vector3d e_p = S_p_ * (x_d_proj - x);
        Eigen::Vector3d edot_p = S_p_ * (xdot_d - xdot);

        Eigen::Vector3d F_t_cmd = S_p_ * (Kp_tan_ * e_p + Kd_tan_ * edot_p);

        // Force loop (normal direction)
        Eigen::Vector3d F_meas = robot_->forceSensor();
        double F_n = n_.dot(F_meas);
        double e_f = Fd_n - F_n;

        double F_n_cmd = Fd_n + kf_ * e_f;
        Eigen::Vector3d F_n_vec = F_n_cmd * n_;

        // Total wrench
        Eigen::Vector3d F_cmd = F_t_cmd + F_n_vec;

        // Map to joint torques and add bias
        Eigen::VectorXd tau = J.transpose() * F_cmd;
        Eigen::VectorXd tau_bias = robot_->inverseDynamics(
            q, qdot, Eigen::VectorXd::Zero(q.size())
        );
        return tau + tau_bias;
    }

private:
    const RobotModel *robot_;
    Eigen::Vector3d n_;
    Eigen::Matrix3d S_p_, S_f_;
    Eigen::Matrix3d Kp_tan_, Kd_tan_;
    double kf_;
};
      

In practice, one must also handle loss of contact (e.g., if the measured normal force drops below a threshold) by switching back to pure position control, which can be integrated via a state machine around this controller.

In Java, one may use a linear algebra library such as EJML for matrix operations. The following sketch assumes a minimal interface RobotModelJava and uses EJML's DMatrixRMaj and CommonOps_DDRM.

import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;

interface RobotModelJava {
    DMatrixRMaj forwardKinematics(DMatrixRMaj q);      // 3x1
    DMatrixRMaj jacobian(DMatrixRMaj q);               // 3xn
    DMatrixRMaj inverseDynamics(DMatrixRMaj q,
                                DMatrixRMaj qdot,
                                DMatrixRMaj qddot);    // nx1
    DMatrixRMaj forceSensor();                         // 3x1
}

public class HybridPositionForceControllerJava {

    private RobotModelJava robot;
    private DMatrixRMaj n;     // 3x1
    private DMatrixRMaj S_p;   // 3x3
    private DMatrixRMaj S_f;   // 3x3
    private DMatrixRMaj Kp_tan;
    private DMatrixRMaj Kd_tan;
    private double kf;

    public HybridPositionForceControllerJava(RobotModelJava robot,
                                             DMatrixRMaj contactNormalWorld) {
        this.robot = robot;
        this.n = contactNormalWorld.copy();
        normalize(n);

        S_f = new DMatrixRMaj(3, 3);
        outer(n, n, S_f);
        S_p = CommonOps_DDRM.identity(3);
        CommonOps_DDRM.subtractEquals(S_p, S_f);

        Kp_tan = CommonOps_DDRM.identity(3);
        CommonOps_DDRM.scale(200.0, Kp_tan);
        Kd_tan = CommonOps_DDRM.identity(3);
        CommonOps_DDRM.scale(40.0, Kd_tan);
        kf = 50.0;
    }

    public DMatrixRMaj computeTorque(DMatrixRMaj q,
                                     DMatrixRMaj qdot,
                                     DMatrixRMaj x_d,
                                     DMatrixRMaj xdot_d,
                                     double Fd_n) {

        DMatrixRMaj x = robot.forwardKinematics(q);
        DMatrixRMaj J = robot.jacobian(q);          // 3xn
        DMatrixRMaj xdot = new DMatrixRMaj(3, 1);
        CommonOps_DDRM.mult(J, qdot, xdot);

        // Project desired position to tangent plane
        DMatrixRMaj delta = new DMatrixRMaj(3, 1);
        CommonOps_DDRM.subtract(x_d, x, delta);
        double nTdelta = dot(n, delta);
        DMatrixRMaj normalComponent = new DMatrixRMaj(3, 1);
        CommonOps_DDRM.scale(nTdelta, n, normalComponent);
        DMatrixRMaj x_d_proj = new DMatrixRMaj(3, 1);
        CommonOps_DDRM.subtract(x_d, normalComponent, x_d_proj);

        DMatrixRMaj e_p = new DMatrixRMaj(3, 1);
        DMatrixRMaj tmp = new DMatrixRMaj(3, 1);
        CommonOps_DDRM.subtract(x_d_proj, x, tmp);
        CommonOps_DDRM.mult(S_p, tmp, e_p);

        DMatrixRMaj e_dot_p = new DMatrixRMaj(3, 1);
        CommonOps_DDRM.subtract(xdot_d, xdot, tmp);
        CommonOps_DDRM.mult(S_p, tmp, e_dot_p);

        DMatrixRMaj F_t_cmd = new DMatrixRMaj(3, 1);
        DMatrixRMaj tmp3 = new DMatrixRMaj(3, 1);
        CommonOps_DDRM.mult(Kp_tan, e_p, F_t_cmd);
        CommonOps_DDRM.multAdd(Kd_tan, e_dot_p, F_t_cmd);
        CommonOps_DDRM.mult(S_p, F_t_cmd, tmp3);
        F_t_cmd.set(tmp3);

        // Force loop
        DMatrixRMaj F_meas = robot.forceSensor();
        double F_n = dot(n, F_meas);
        double e_f = Fd_n - F_n;
        double F_n_cmd = Fd_n + kf * e_f;

        DMatrixRMaj F_n_vec = new DMatrixRMaj(3, 1);
        CommonOps_DDRM.scale(F_n_cmd, n, F_n_vec);

        DMatrixRMaj F_cmd = new DMatrixRMaj(3, 1);
        CommonOps_DDRM.add(F_t_cmd, F_n_vec, F_cmd);

        // Map to joint torques tau = J^T F_cmd + bias
        DMatrixRMaj tau = new DMatrixRMaj(q.getNumRows(), 1);
        CommonOps_DDRM.multTransA(J, F_cmd, tau);

        DMatrixRMaj qddotZero = new DMatrixRMaj(q.getNumRows(), 1);
        DMatrixRMaj tauBias = robot.inverseDynamics(q, qdot, qddotZero);
        CommonOps_DDRM.addEquals(tau, tauBias);

        return tau;
    }

    // Helper methods
    private static void normalize(DMatrixRMaj v) {
        double norm = Math.sqrt(dot(v, v));
        CommonOps_DDRM.scale(1.0 / norm, v);
    }

    private static double dot(DMatrixRMaj a, DMatrixRMaj b) {
        double s = 0.0;
        for (int i = 0; i < a.getNumRows(); ++i) {
            s += a.get(i, 0) * b.get(i, 0);
        }
        return s;
    }

    private static void outer(DMatrixRMaj a, DMatrixRMaj b, DMatrixRMaj out) {
        for (int i = 0; i < 3; ++i) {
            for (int j = 0; j < 3; ++j) {
                out.set(i, j, a.get(i, 0) * b.get(j, 0));
            }
        }
    }
}
      

The Java implementation mirrors the Python and C++ versions, emphasizing that hybrid position/force control is essentially a linear algebra computation once the geometric quantities (Jacobian, contact normal) and sensor data are available.

9.1 MATLAB Function for Hybrid Control

MATLAB's Robotics System Toolbox provides kinematics and dynamics, and Simulink can host the control loop. A basic MATLAB function computing the torque is:

function tau = hybridPositionForceControl(q, qdot, x_d, xdot_d, Fd_n, robot, n_world, Kp_tan, Kd_tan, kf)
%HYBRIDPOSITIONFORCECONTROL Compute joint torques for hybrid position/force control.
%  q, qdot   : joint state (nx1)
%  x_d, xdot_d : desired end-effector position/velocity (3x1)
%  Fd_n      : desired normal force (scalar)
%  robot     : robot model with forwardKinematics, jacobian, inverseDynamics
%  n_world   : 3x1 unit normal vector (world frame)

if nargin < 8 || isempty(Kp_tan)
    Kp_tan = 200 * eye(3);
end
if nargin < 9 || isempty(Kd_tan)
    Kd_tan = 40 * eye(3);
end
if nargin < 10 || isempty(kf)
    kf = 50;
end

n = n_world(:) / norm(n_world);
S_f = n * n.';
S_p = eye(3) - S_f;

x = robot.forwardKinematics(q);   % 3x1
J = robot.jacobian(q);            % 3xn
xdot = J * qdot;

% Project desired position onto tangent plane
delta = x_d - x;
normal_component = (n.' * delta) * n;
x_d_proj = x_d - normal_component;

e_p = S_p * (x_d_proj - x);
edot_p = S_p * (xdot_d - xdot);

F_t_cmd = S_p * (Kp_tan * e_p + Kd_tan * edot_p);

F_meas = robot.forceSensor();     % 3x1
F_n = n.' * F_meas;
e_f = Fd_n - F_n;
F_n_cmd = Fd_n + kf * e_f;
F_n_vec = F_n_cmd * n;

F_cmd = F_t_cmd + F_n_vec;

tau = J.' * F_cmd;
tau_bias = robot.inverseDynamics(q, qdot, zeros(size(q)));
tau = tau + tau_bias;
end
      

In Simulink, one typically:

• Places blocks computing \( \mathbf{x}, \dot{\mathbf{x}}, \mathbf{J} \) from \( \mathbf{q}, \dot{\mathbf{q}} \).
• Implements the matrix operations for \( \mathbf{S}_p, \mathbf{S}_f \), the PD/force loops, and the torque mapping.
• Uses a torque source block to apply \( \boldsymbol{\tau} \) to the multibody model.

9.2 Wolfram Mathematica: Symbolic and Numeric Hybrid Control

Mathematica is convenient for symbolic verification of selection matrices and for simulating simplified dynamics. Below is a sketch for a point mass in 1D normal direction with force control:

(* Parameters *)
m = 1.0;
ke = 1000.0;   (* environment stiffness in normal direction *)
kf = 50.0;     (* force feedback gain *)
Fd = 10.0;     (* desired normal force *)

(* State variables: x normal displacement, v velocity *)
x[t_] := x[t];
v[t_] := x'[t];

(* Environment force (linear spring) *)
Fenv[x_] := ke * x;

(* Force control law: Fn_cmd = Fd + kf*(Fd - Fenv) *)
FnCmd[x_] := Fd + kf * (Fd - Fenv[x]);

(* Dynamics: m * x'' = FnCmd - Fenv (ignoring tangential directions) *)
de = m * x''[t] == FnCmd[x[t]] - Fenv[x[t]];

ic = {x[0] == 0.0, x'[0] == 0.0};

sol = NDSolve[{de, ic}, x, {t, 0, 2.0}][[1]];

Plot[
  {Fenv[x[t] /. sol], FnCmd[x[t] /. sol]},
  {t, 0, 2.0},
  PlotLegends -> {"F_env", "F_cmd"}
]
      

For a full manipulator, one can symbolically define \( \mathbf{S}_p, \mathbf{S}_f, \mathbf{J}, \mathbf{M} \) and verify identities such as \( \mathbf{S}_p^2 = \mathbf{S}_p \), \( \mathbf{S}_p \mathbf{S}_f = \mathbf{0} \), and the decoupling of tangent/normal dynamics under ideal contact assumptions.

In this lesson we formulated hybrid position/force control for robots interacting with constrained environments. Starting from operational-space dynamics, we introduced selection matrices \( \mathbf{S}_p \) and \( \mathbf{S}_f \) as complementary projectors defining motion- and force-controlled subspaces. The hybrid law combines:

• Position (or acceleration) control in the tangent subspace, ensuring tracking of desired trajectories.
• Force control in the normal subspace, regulating contact forces to desired profiles.

Under ideal conditions (rigid contact, appropriate frame alignment, and block-diagonal inertia), the tangent and normal dynamics decouple, yielding interpretable and tunable loops. We provided concrete implementations in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica, and analyzed simplified models to understand loop stability. These foundations will be further generalized in the next lessons on impedance and admittance control, which reinterpret position/force control in terms of desired dynamic interaction behaviors.