Consider two or more manipulators rigidly grasping the same rigid object and moving it along a desired trajectory while a team of mobile manipulators executes several pick-and-place tasks in parallel. Our lab separates the problem into:

• Object-level cooperative manipulation: compute contact forces and joint velocities that realize a desired object wrench and twist while respecting rigid grasp constraints.
• Team task allocation: assign tasks to robots (or robot teams) by solving a constrained optimization problem over a cost matrix.

We assume you are already comfortable with single-robot kinematics and dynamics (configuration, Jacobian, wrench, and rigid-body equations of motion). We now define a stacked state vector

\[ \mathbf{x} = \begin{bmatrix} \mathbf{x}_o^\top & \mathbf{q}_1^\top & \dots & \mathbf{q}_N^\top \end{bmatrix}^\top, \]

where \( \mathbf{x}_o \in \mathbb{R}^6 \) encodes the object pose (for instance as position and minimal orientation coordinates) and \( \mathbf{q}_i \) is the joint configuration of robot \( i \). Our controller will operate at the level of desired \( \mathbf{w}_o \) (object wrench) and \( \mathbf{v}_o \) (object twist).

Let the object coordinate frame be attached to its center of mass. At contact \( i \) we denote by \( \mathbf{p}_i \in \mathbb{R}^3 \) the vector from the object frame origin to the contact point, and by \( \mathbf{f}_i \in \mathbb{R}^3 \) the contact force expressed in the object frame. The corresponding wrench is

\[ \mathbf{w}_i = \begin{bmatrix} \mathbf{f}_i \\ \mathbf{m}_i \end{bmatrix} = \begin{bmatrix} \mathbf{f}_i \\ \mathbf{p}_i \times \mathbf{f}_i \end{bmatrix} \in \mathbb{R}^6. \]

For \( m \) contacts we stack all forces into \( \mathbf{f}_c \in \mathbb{R}^{3m} \), \( \mathbf{f}_c^\top = [\mathbf{f}_1^\top, \dots, \mathbf{f}_m^\top] \). The total object wrench is

\[ \mathbf{w}_o = \mathbf{G}\,\mathbf{f}_c, \quad \mathbf{G} \in \mathbb{R}^{6 \times 3m}, \]

where \( \mathbf{G} \) is the grasp matrix, obtained by concatenating the contribution of each contact,

\[ \mathbf{G} = \begin{bmatrix} \mathbf{I}_3 & \dots & \mathbf{I}_3 \\ [\mathbf{p}_1]_\times & \dots & [\mathbf{p}_m]_\times \end{bmatrix}, \]

and \( [\mathbf{p}_i]_\times \) is the skew-symmetric matrix such that \( [\mathbf{p}_i]_\times \mathbf{f}_i = \mathbf{p}_i \times \mathbf{f}_i \).

For a desired object wrench \( \mathbf{w}_o^{\mathrm{des}} \), any vector of contact forces satisfying

\[ \mathbf{G}\,\mathbf{f}_c = \mathbf{w}_o^{\mathrm{des}} \]

produces the same macroscopic effect on the object. The set of feasible \( \mathbf{f}_c \) can be decomposed into a particular solution plus internal forces that do not affect the object:

\[ \mathbf{f}_c = \mathbf{f}_p + \mathbf{N}_G \,\boldsymbol{\lambda}, \quad \mathbf{G}\,\mathbf{f}_p = \mathbf{w}_o^{\mathrm{des}}, \quad \mathbf{G}\,\mathbf{N}_G = \mathbf{0}, \]

where the columns of \( \mathbf{N}_G \) form a basis for the nullspace of \( \mathbf{G} \). Any \( \mathbf{f}_\mathrm{int} = \mathbf{N}_G \boldsymbol{\lambda} \) satisfies \( \mathbf{G}\,\mathbf{f}_\mathrm{int} = \mathbf{0} \), and thus does not change \( \mathbf{w}_o \); these are internal forces that can be used, for example, to regulate grasp stability or contact friction margins.

A common design is to choose the vector of contact forces that minimizes a quadratic cost while exactly realizing the desired object wrench. Let \( \mathbf{W} \in \mathbb{R}^{3m \times 3m} \) be symmetric positive definite, weighting directions or contacts. We solve

\[ \begin{aligned} \mathbf{f}_c^\star &= \arg\min_{\mathbf{f}_c} \tfrac{1}{2}\,\mathbf{f}_c^\top \mathbf{W}\,\mathbf{f}_c \\ &\text{subject to } \mathbf{G}\,\mathbf{f}_c = \mathbf{w}_o^{\mathrm{des}}. \end{aligned} \]

Introducing Lagrange multipliers \( \boldsymbol{\lambda} \in \mathbb{R}^6 \) for the wrench constraint, the Lagrangian is

\[ \mathcal{L}(\mathbf{f}_c,\boldsymbol{\lambda}) = \tfrac{1}{2}\,\mathbf{f}_c^\top \mathbf{W}\,\mathbf{f}_c + \boldsymbol{\lambda}^\top (\mathbf{G}\,\mathbf{f}_c - \mathbf{w}_o^{\mathrm{des}}). \]

Stationarity with respect to \( \mathbf{f}_c \) and \( \boldsymbol{\lambda} \) yields

\[ \begin{aligned} \frac{\partial \mathcal{L}}{\partial \mathbf{f}_c} &= \mathbf{W}\,\mathbf{f}_c + \mathbf{G}^\top \boldsymbol{\lambda} = \mathbf{0}, \\ \frac{\partial \mathcal{L}}{\partial \boldsymbol{\lambda}} &= \mathbf{G}\,\mathbf{f}_c - \mathbf{w}_o^{\mathrm{des}} = \mathbf{0}. \end{aligned} \]

From the first equation, \( \mathbf{f}_c = -\mathbf{W}^{-1}\mathbf{G}^\top \boldsymbol{\lambda} \). Substituting into the constraint gives

\[ -\mathbf{G}\,\mathbf{W}^{-1}\mathbf{G}^\top \boldsymbol{\lambda} = \mathbf{w}_o^{\mathrm{des}}, \quad \Rightarrow \quad \boldsymbol{\lambda} = -(\mathbf{G}\,\mathbf{W}^{-1}\mathbf{G}^\top)^{-1} \mathbf{w}_o^{\mathrm{des}}. \]

Finally,

\[ \mathbf{f}_c^\star = \mathbf{W}^{-1}\mathbf{G}^\top (\mathbf{G}\,\mathbf{W}^{-1}\mathbf{G}^\top)^{-1} \mathbf{w}_o^{\mathrm{des}}. \]

Because \( \mathbf{W} \) is positive definite and \( \mathbf{G} \) has full row rank (no force closure degeneracy), the matrix \( \mathbf{G}\,\mathbf{W}^{-1}\mathbf{G}^\top \) is positive definite, and thus the optimal forces are unique. In practice one adds friction cone inequality constraints and force bounds, turning the equality-constrained problem into a quadratic program; here we focus on the analytically tractable core.

Let \( \mathbf{v}_o \in \mathbb{R}^6 \) be the spatial twist of the object (linear and angular velocity expressed in the object frame). For each robot \( i \), the contact twist equals the end-effector twist, \( \mathbf{v}_{\mathrm{ee},i} = \mathbf{J}_i(\mathbf{q}_i)\,\dot{\mathbf{q}}_i \), and is related to the object twist by a constant transform (as long as the grasp is rigid):

\[ \mathbf{v}_{\mathrm{ee},i} = \mathbf{J}_{oi}(\mathbf{x}_o)\,\mathbf{v}_o, \quad i = 1,\dots,N. \]

Imposing rigid grasp constraints,

\[ \mathbf{J}_i(\mathbf{q}_i)\,\dot{\mathbf{q}}_i - \mathbf{J}_{oi}(\mathbf{x}_o)\,\mathbf{v}_o = \mathbf{0}. \]

Stacking all robots into a single linear system \( \mathbf{A}\,\dot{\mathbf{q}} = \mathbf{b} \), with \( \dot{\mathbf{q}}^\top = [\dot{\mathbf{q}}_1^\top,\dots,\dot{\mathbf{q}}_N^\top] \) and

\[ \mathbf{A} = \begin{bmatrix} \mathbf{J}_1(\mathbf{q}_1) & \dots & \mathbf{0} \\ \vdots & \ddots & \vdots \\ \mathbf{0} & \dots & \mathbf{J}_N(\mathbf{q}_N) \end{bmatrix}, \quad \mathbf{b} = \begin{bmatrix} \mathbf{J}_{o1}(\mathbf{x}_o)\,\mathbf{v}_o \\ \vdots \\ \mathbf{J}_{oN}(\mathbf{x}_o)\,\mathbf{v}_o \end{bmatrix}, \]

the minimum norm joint-velocity solution is

\[ \dot{\mathbf{q}}^\star = \mathbf{A}^\top (\mathbf{A}\,\mathbf{A}^\top)^{-1} \mathbf{b}, \]

obtained again by Lagrange multipliers or by interpreting the problem as a least-squares projection onto the constraint manifold. In many cooperative controllers, this velocity-level solution is combined with force distribution (Section 3) in an operational-space control law.

We now consider \( n \) robots and \( m \) tasks. Each pair \( (i,j) \) (robot \( i \), task \( j \)) is assigned a cost \( c_{ij} \in \mathbb{R} \) (e.g., expected execution time, path length, or energy). We introduce binary decision variables \( x_{ij} \in \{0,1\} \), with \( x_{ij} = 1 \) if robot \( i \) executes task \( j \), and formulate the assignment problem:

\[ \begin{aligned} \min_{\{x_{ij}\}} \quad & \sum_{i=1}^n \sum_{j=1}^m c_{ij}\,x_{ij} \\ \text{subject to} \quad & \sum_{j=1}^m x_{ij} \leq 1, \quad i=1,\dots,n, \\ & \sum_{i=1}^n x_{ij} = 1, \quad j=1,\dots,m, \\ & x_{ij} \in \{0,1\}. \end{aligned} \]

The first constraint ensures that each robot is assigned at most one task; the second ensures that each task is assigned to exactly one robot. This is the classical linear assignment problem, whose constraint matrix is totally unimodular; the linear programming relaxation with \( 0 \leq x_{ij} \leq 1 \) already produces integral solutions.

For tasks that require cooperative manipulation (e.g., tasks that involve the shared object), we model each team as a compound agent: a subset of robots that must be allocated together. The same formulation applies, but now rows correspond to robot teams, not individual robots. The lab will use a centralized solver, but the cost structure can be used in distributed auction algorithms as well.

The following diagram summarizes the flow of computations in this lab: from defining object and contact geometry, through task allocation, to cooperative control at the wrench and velocity levels.

flowchart TD
  S["Start lab"] --> G1["Define robots, object model, contact points"]
  G1 --> C1["Compute cost matrix C_ij for robots and tasks"]
  C1 --> A1["Solve assignment problem (centralized)"]
  A1 --> T1["Form cooperative teams for object tasks"]
  T1 --> F1["Build grasp matrix G and desired object wrench"]
  F1 --> F2["Solve optimal contact forces using GWG inverse"]
  F2 --> K1["Compute joint velocities from velocity constraints"]
  K1 --> SIM["Simulate trajectories and log results"]
        

We start with a planar model: a rigid object in the plane subject to two contact forces. The object wrench has three components \( \mathbf{w}_o = [F_x, F_y, M_z]^\top \), and each contact provides a 2D force. The planar grasp matrix for contact positions \( (x_i,y_i) \) is

\[ \mathbf{G}_\mathrm{planar} = \begin{bmatrix} 1 & 0 & 1 & 0 \\ 0 & 1 & 0 & 1 \\ -y_1 & x_1 & -y_2 & x_2 \end{bmatrix}. \]

The code below computes the grasp matrix, solves the optimal contact forces for a given desired wrench, and then solves the assignment problem using SciPy's linear_sum_assignment. In a robotics stack you would embed this into a ROS 2 node with rclpy and, for manipulators, connect to MoveIt via moveit_commander.

import numpy as np
from scipy.optimize import linear_sum_assignment

# ---------- Cooperative planar grasp ----------

def planar_grasp_matrix(contact_points):
    """
    contact_points: array of shape (m, 2) with rows [x_i, y_i].
    Returns G in R^{3 x 2m}.
    """
    contact_points = np.asarray(contact_points, dtype=float)
    m = contact_points.shape[0]
    G = np.zeros((3, 2 * m))
    for i in range(m):
        x_i, y_i = contact_points[i]
        col = 2 * i
        # Force components
        G[0, col] = 1.0      # f_x_i
        G[1, col + 1] = 1.0  # f_y_i
        # Moment around z: tau_z_i = x_i * f_y_i - y_i * f_x_i
        G[2, col] = -y_i
        G[2, col + 1] = x_i
    return G

# Two symmetric contacts on a bar of length 1.0
contacts = np.array([[0.5, 0.0],
                     [-0.5, 0.0]])

G = planar_grasp_matrix(contacts)
m = contacts.shape[0]
W = np.eye(2 * m)

# Desired wrench: support gravity of an object of mass m_obj at rest
m_obj = 2.0    # kg
g = 9.81       # m/s^2
w_des = np.array([0.0, m_obj * g, 0.0])

# Solve WLS: f_c* = W^{-1} G^T (G W^{-1} G^T)^{-1} w_des
GWG = G @ np.linalg.inv(W) @ G.T
lambda_vec = np.linalg.solve(GWG, w_des)
f_c_star = np.linalg.inv(W) @ G.T @ lambda_vec

print("Grasp matrix G:\n", G)
print("Optimal contact forces f_c*:", f_c_star)

# ---------- Team task allocation via linear_sum_assignment ----------

# Example: 3 robots, 3 tasks, cost = approximate execution time in seconds
C = np.array([
    [4.0, 2.0, 3.5],
    [2.5, 3.0, 2.0],
    [3.0, 4.0, 1.5]
])

row_ind, col_ind = linear_sum_assignment(C)
total_cost = C[row_ind, col_ind].sum()

print("Assignment (robot_i, task_j):")
for i, j in zip(row_ind, col_ind):
    print(f"  robot {i} -> task {j}, cost = {C[i, j]}")

print("Total cost:", total_cost)
      

To extend this to full 3D manipulators, replace planar_grasp_matrix by a 3D grasp matrix computation and connect \( \mathbf{f}_c^\star \) to joint torques using the Jacobian transpose mapping \( \mathbf{u}_i = \mathbf{J}_i(\mathbf{q}_i)^\top \mathbf{f}_i \).

In C++, a natural choice is to use Eigen for linear algebra and integrate the code into a ROS 2 node (with rclcpp) that subscribes to object pose and publishes target joint torques. Below is a minimalist example showing the planar grasp matrix and a simple greedy (suboptimal but short) allocation algorithm.

#include <Eigen/Dense>
#include <vector>
#include <iostream>

using Eigen::MatrixXd;
using Eigen::VectorXd;

MatrixXd planarGraspMatrix(const std::vector<Eigen::Vector2d> &contacts)
{
    int m = static_cast<int>(contacts.size());
    MatrixXd G(3, 2 * m);
    G.setZero();
    for (int i = 0; i < m; ++i) {
        double x = contacts;
        double y = contacts;
        int col = 2 * i;
        G(0, col)     = 1.0;
        G(1, col + 1) = 1.0;
        G(2, col)     = -y;
        G(2, col + 1) = x;
    }
    return G;
}

int main()
{
    // Example contacts
    std::vector<Eigen::Vector2d> contacts;
    contacts.emplace_back(0.5, 0.0);
    contacts.emplace_back(-0.5, 0.0);

    MatrixXd G = planarGraspMatrix(contacts);
    int m = static_cast<int>(contacts.size());
    MatrixXd W = MatrixXd::Identity(2 * m, 2 * m);

    double m_obj = 2.0;
    double g = 9.81;
    VectorXd w_des(3);
    w_des << 0.0, m_obj * g, 0.0;

    MatrixXd GWG = G * W.inverse() * G.transpose();
    VectorXd lambda_vec = GWG.ldlt().solve(w_des);
    VectorXd f_c_star = W.inverse() * G.transpose() * lambda_vec;

    std::cout << "Optimal forces f_c* = " << f_c_star.transpose() << std::endl;

    // ---------- Greedy task allocation (for small problems) ----------
    const int nRobots = 3;
    const int nTasks  = 3;
    double C[nRobots][nTasks] = {
        {4.0, 2.0, 3.5},
        {2.5, 3.0, 2.0},
        {3.0, 4.0, 1.5}
    };

    std::vector<int> taskOfRobot(nRobots, -1);
    std::vector<bool> taskTaken(nTasks, false);

    for (int i = 0; i < nRobots; ++i) {
        double best = 1e9;
        int bestTask = -1;
        for (int j = 0; j < nTasks; ++j) {
            if (taskTaken[j]) {
                continue;
            }
            if (C[i][j] < best) {
                best = C[i][j];
                bestTask = j;
            }
        }
        if (bestTask != -1) {
            taskOfRobot[i] = bestTask;
            taskTaken[bestTask] = true;
        }
    }

    for (int i = 0; i < nRobots; ++i) {
        std::cout << "robot " << i
                  << " -> task " << taskOfRobot[i]
                  << std::endl;
    }

    return 0;
}
      

For a production-quality lab, replace the greedy allocator with an open-source Hungarian implementation, and map contact forces to joint torques via \( \mathbf{u}_i = \mathbf{J}_i^\top \mathbf{f}_i \) using the manipulator's Jacobian from a library such as pinocchio or the kinematics functions provided by your robot manufacturer.

Java is frequently used for high-level coordination in multi-agent systems (e.g., with JADE-like agent frameworks). Here we demonstrate a compact assignment implementation. In the lab, this component can publish allocations over a middleware (e.g., ROS 2 Java client) to manipulators running lower-level controllers.

public class TeamAssignment {

    public static int[] greedyAssign(double[][] C) {
        int nRobots = C.length;
        int nTasks = C[0].length;
        int[] taskOfRobot = new int[nRobots];
        boolean[] taskTaken = new boolean[nTasks];

        for (int i = 0; i < nRobots; ++i) {
            double best = Double.POSITIVE_INFINITY;
            int bestTask = -1;
            for (int j = 0; j < nTasks; ++j) {
                if (taskTaken[j]) {
                    continue;
                }
                if (C[i][j] < best) {
                    best = C[i][j];
                    bestTask = j;
                }
            }
            taskOfRobot[i] = bestTask;
            if (bestTask != -1) {
                taskTaken[bestTask] = true;
            }
        }
        return taskOfRobot;
    }

    public static void main(String[] args) {
        double[][] C = {
            {4.0, 2.0, 3.5},
            {2.5, 3.0, 2.0},
            {3.0, 4.0, 1.5}
        };
        int[] assignment = greedyAssign(C);
        for (int i = 0; i < assignment.length; ++i) {
            System.out.println("robot " + i + " -> task " + assignment[i]);
        }
    }
}
      

To incorporate cooperative manipulation, you can expose Java interfaces that send desired object wrenches and twists to lower-level controllers (implemented in C++ or Python) after the assignment step has determined which robots participate in each cooperative task.

MATLAB with the Robotics System Toolbox is convenient for rapid prototyping of cooperative manipulation controllers and for generating Simulink diagrams. The script below mirrors the planar example, and then uses matchpairs for assignment (available in some MATLAB toolboxes).

function cooperative_planar_lab()
    % Contact points on a bar
    contacts = [0.5, 0.0;
                -0.5, 0.0];

    G = planarGraspMatrix(contacts);
    m = size(contacts, 1);
    W = eye(2 * m);

    m_obj = 2.0;
    g = 9.81;
    w_des = [0.0; m_obj * g; 0.0];

    GWG = G / W * G';
    lambda_vec = GWG \ w_des;
    f_c_star = (W \ G') * lambda_vec;

    disp('Grasp matrix G = ');
    disp(G);
    disp('Optimal contact forces f_c* = ');
    disp(f_c_star);

    % Task allocation example
    C = [4.0, 2.0, 3.5;
         2.5, 3.0, 2.0;
         3.0, 4.0, 1.5];

    % matchpairs solves a linear assignment problem
    [pairs, totalCost] = matchpairs(C, 1e3);
    disp('Assignments [robot, task]:');
    disp(pairs);
    disp(['Total cost = ', num2str(totalCost)]);
end

function G = planarGraspMatrix(contacts)
    m = size(contacts, 1);
    G = zeros(3, 2 * m);
    for i = 1:m
        x_i = contacts(i, 1);
        y_i = contacts(i, 2);
        col = 2 * (i - 1) + 1;
        G(1, col)     = 1.0;
        G(2, col + 1) = 1.0;
        G(3, col)     = -y_i;
        G(3, col + 1) = x_i;
    end
end
      

In Simulink, you can represent the grasp matrix multiplication and the object dynamics in separate subsystems: one subsystem computes \( \mathbf{f}_c^\star \) from \( \mathbf{w}_o^{\mathrm{des}} \), another integrates the object motion, and a third subsystem distributes joint torques to each manipulator based on their Jacobians.

Wolfram Mathematica is well suited for verifying symbolic properties of grasp matrices and doing quick numerical experiments. The following code constructs the planar grasp matrix for two contacts, verifies the internal-force nullspace, and solves the weighted least-squares force distribution.

(* Define planar contacts *)
contacts = { {0.5, 0.0}, {-0.5, 0.0} };
m = Length[contacts];

(* Build planar grasp matrix G in R^{3 x 2m} *)
G = ConstantArray[0.0, {3, 2 m}];
Do[
  {x, y} = contacts[[i]];
  col = 2 i - 1;
  G[[1, col]] = 1.0;
  G[[2, col + 1]] = 1.0;
  G[[3, col]] = -y;
  G[[3, col + 1]] = x;
, {i, 1, m}];

G // MatrixForm

(* Weight matrix and desired wrench *)
W = IdentityMatrix[2 m];
mObj = 2.0;
g = 9.81;
wDes = {0.0, mObj g, 0.0};

GWG = G . Inverse[W] . Transpose[G];
lambdaVec = LinearSolve[GWG, wDes];
fCStar = Inverse[W] . Transpose[G] . lambdaVec

(* Verify that any internal force in Null[G] does not affect the wrench *)
internalBasis = NullSpace[G];
If[Length[internalBasis] > 0,
  With[{fInt = internalBasis[[1]]},
    Simplify[G . fInt]
  ],
  "No nontrivial internal forces (full column rank)."
]
      

You can extend this notebook to three-dimensional grasps, include friction cone approximations, and link to Mathematica's LinearProgramming function to solve assignment problems and compare their solutions with your own implementations.

Problem 1 (Internal Forces and Nullspace): Let \( \mathbf{G} \in \mathbb{R}^{6 \times 3m} \) be the grasp matrix for a rigid object. Show that any \( \mathbf{f}_\mathrm{int} \in \mathbb{R}^{3m} \) satisfying \( \mathbf{G}\,\mathbf{f}_\mathrm{int} = \mathbf{0} \) does not affect the object wrench, and argue why the dimension of the internal force space is \( 3m - \operatorname{rank}(\mathbf{G}) \).

Solution: The object wrench is \( \mathbf{w}_o = \mathbf{G}\,\mathbf{f}_c \). Suppose we change the contact forces by adding \( \mathbf{f}_\mathrm{int} \): \( \tilde{\mathbf{f}}_c = \mathbf{f}_c + \mathbf{f}_\mathrm{int} \). Then the new wrench is

\[ \tilde{\mathbf{w}}_o = \mathbf{G}\,\tilde{\mathbf{f}}_c = \mathbf{G}\,\mathbf{f}_c + \mathbf{G}\,\mathbf{f}_\mathrm{int} = \mathbf{w}_o + \mathbf{0} = \mathbf{w}_o, \]

so the macroscopic effect on the object is unchanged. The internal force space is the nullspace \( \mathcal{N}(\mathbf{G}) \). By the rank–nullity theorem, for a linear map \( \mathbf{G}:\mathbb{R}^{3m} \to \mathbb{R}^6 \) we have \( \dim \mathcal{N}(\mathbf{G}) + \operatorname{rank}(\mathbf{G}) = 3m \), hence \( \dim \mathcal{N}(\mathbf{G}) = 3m - \operatorname{rank}(\mathbf{G}) \).

Problem 2 (Minimal-Norm Force Solution): Consider the equality-constrained problem in Section 3 with \( \mathbf{W} = \mathbf{I} \). Show that the optimal solution is \( \mathbf{f}_c^\star = \mathbf{G}^\top(\mathbf{G}\mathbf{G}^\top)^{-1} \mathbf{w}_o^{\mathrm{des}} \), and that this is the unique solution of minimal Euclidean norm among all feasible \( \mathbf{f}_c \).

Solution: With \( \mathbf{W} = \mathbf{I} \), the KKT system in Section 3 simplifies to

\[ \mathbf{f}_c + \mathbf{G}^\top \boldsymbol{\lambda} = \mathbf{0}, \quad \mathbf{G}\,\mathbf{f}_c = \mathbf{w}_o^{\mathrm{des}}. \]

Eliminating \( \mathbf{f}_c \) gives \( -\mathbf{G}\mathbf{G}^\top \boldsymbol{\lambda} = \mathbf{w}_o^{\mathrm{des}} \), so \( \boldsymbol{\lambda} = -(\mathbf{G}\mathbf{G}^\top)^{-1}\mathbf{w}_o^{\mathrm{des}} \), and

\[ \mathbf{f}_c^\star = -\mathbf{G}^\top \boldsymbol{\lambda} = \mathbf{G}^\top(\mathbf{G}\mathbf{G}^\top)^{-1} \mathbf{w}_o^{\mathrm{des}}. \]

Any other feasible force vector can be written as \( \mathbf{f}_c = \mathbf{f}_c^\star + \mathbf{f}_\mathrm{int} \) with \( \mathbf{f}_\mathrm{int} \in \mathcal{N}(\mathbf{G}) \). Then \( \|\mathbf{f}_c\|_2^2 = \|\mathbf{f}_c^\star\|_2^2 + \|\mathbf{f}_\mathrm{int}\|_2^2 \), because \( \mathbf{f}_c^\star \) lies in the row space of \( \mathbf{G}^\top \) while \( \mathbf{f}_\mathrm{int} \) lies in the nullspace of \( \mathbf{G} \), and these subspaces are orthogonal. Therefore \( \mathbf{f}_c^\star \) is the unique minimal-norm solution.

Problem 3 (Planar Grasp Matrix Construction): For a planar object with two contacts at \( (x_1,y_1) = (0.5,0) \) and \( (x_2,y_2) = (-0.5,0) \), derive explicitly the planar grasp matrix \( \mathbf{G}_\mathrm{planar} \) and verify that a pure upward wrench \( \mathbf{w}_o^{\mathrm{des}} = [0,F,0]^\top \) can be achieved by equal-and-opposite contact forces.

Solution: Using the general planar form from Section 7,

\[ \mathbf{G}_\mathrm{planar} = \begin{bmatrix} 1 & 0 & 1 & 0 \\ 0 & 1 & 0 & 1 \\ -y_1 & x_1 & -y_2 & x_2 \end{bmatrix} = \begin{bmatrix} 1 & 0 & 1 & 0 \\ 0 & 1 & 0 & 1 \\ 0 & 0.5 & 0 & -0.5 \end{bmatrix}. \]

Let \( \mathbf{f}_c = [f_{x1}, f_{y1}, f_{x2}, f_{y2}]^\top \). A pure upward wrench satisfies \( f_{x1} + f_{x2} = 0 \), \( f_{y1} + f_{y2} = F \), and \( 0.5 f_{y1} - 0.5 f_{y2} = 0 \). The last equation gives \( f_{y1} = f_{y2} \). Combining with \( f_{y1} + f_{y2} = F \) yields \( f_{y1} = f_{y2} = F/2 \). The first equation admits infinitely many solutions; choosing \( f_{x1} = -f_{x2} \) produces pure internal forces in the horizontal direction, which do not change \( \mathbf{w}_o \). A simple choice is \( f_{x1} = f_{x2} = 0 \), giving equal vertical forces of magnitude \( F/2 \) at each contact.

Problem 4 (Small Assignment Instance): Two robots and two tasks have costs \( c_{11} = 1, c_{12} = 3, c_{21} = 2, c_{22} = 1 \). List all feasible assignments and compute the optimal one.

Solution: There are only two bijective assignments:

• Assignment A: robot 1 → task 1, robot 2 → task 2, total cost \( 1 + 1 = 2 \).
• Assignment B: robot 1 → task 2, robot 2 → task 1, total cost \( 3 + 2 = 5 \).

Therefore the optimal solution is Assignment A. This illustrates the linear assignment formulation: minimization over all permutations of tasks assigned to robots.

Problem 5 (Conceptual Flow): Sketch the conceptual steps you would take when debugging a cooperative manipulation experiment where the object does not follow the commanded trajectory, even though each individual robot appears to track its joint commands.

Solution (high-level flow):

flowchart TD
  D0["Start diagnosis"] --> D1["Check object model and mass properties"]
  D1 --> D2["Verify grasp matrix G and contact locations"]
  D2 --> D3["Plot sum of contact wrenches vs desired wrench"]
  D3 --> D4["Check velocity constraints J_i qdot_i == J_oi v_o"]
  D4 --> D5["Inspect task allocation: correct robots in cooperative team?"]
  D5 --> D6["Adjust controllers or allocation and re-test"]
        

The key is to move from object-level inconsistencies (mismatch between desired and realized wrench or twist) down to contact modeling, Jacobians, and finally the higher-level allocation that decides which robots participate in the cooperative task.

In this lab you combined two central ideas in multi-robot coordination. At the manipulation level, the grasp matrix and weighted least-squares force distribution provide a principled method for generating contact forces that realize a desired object wrench while characterizing internal forces via the nullspace of \( \mathbf{G} \). At the team level, the assignment problem provides a mathematically clean way to allocate tasks to robots or robot teams, with clear optimal solutions and strong linear-programming structure.

The multi-language implementations (Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica) demonstrate how the same mathematical core appears in different software stacks that you will encounter in advanced robotics research and practice. In later work you can replace centralized allocation by distributed auctions and extend the cooperative controllers to full 3D dynamics with friction cones and robust control.