Planning Human-Robot Co-manipulation with Human Motor Control Objectives and Multi-component Reaching Strategies

In plain words

When a person and a robot carry something together, the robot has to move the way the person expects, or the two fight each other. This paper borrows models from human movement science: people reach toward a target quickly but must slow down to be accurate, and they trade the effort of moving against the value of finishing sooner. The authors write these trade-offs as a score and let a computer find the smoothest motion that scores best.

The same score is used in two ways. First, the robot can guess where the person is heading from the first moments of motion and plan the rest of the path toward that guessed goal. Second, when a task has a fast transport part followed by a fine positioning part, the robot can predict the point where a person would naturally switch from the fast part to the careful part, and hand over control there. The experiments are small, with one operator on a robot arm, so they show that the motions look plausible rather than that they are measurably better for people.

TL;DR

• Human reaching is modelled as minimising a speed-accuracy plus cost-benefit objective, then solved by numerical trajectory optimisation §2.1 §3.3.
• Goal uncertainty and state noise are handled in closed form through a Gaussian reward §3.2 eq. 6.
• The planner estimates the human goal from the first 0.2 s of motion and reproduces the human-like velocity asymmetry in simulation §4.3 §3.4.
• Hand-over of authority is placed at a transition point predicted by a Gaussian process fitted to earlier human data §2.2 fig. 2.
• Evidence is qualitative: one operator, five trials per goal, no baseline controller §5.1 §5.2.

Problem

For a robot to collaborate with a person, it has to predict and match human motion. Machine-learned models need interaction data that may not be available, and ergonomic musculoskeletal models do not predict motion §1. The authors state that existing robot planners based on human movement use optimal control, goal inference or minimum-jerk profiles, and that none combines the speed-accuracy and cost-benefit trade-offs with a sub-movement strategy §1.

The planner assumes point-to-point reaching, a linear Gaussian approximation of the human dynamics for the closed-form expectation §3.1, and a goal whose position is uncertain. Nonlinear dynamics are handled by the numerical solver, at a cost of more than one second for a 7-DoF arm §4.2 tab. 1. The paper lists its own limitations: only point-to-point motions, no interaction dynamics, an initial guess for the goal, a solve time above one second, and tuned weights §6.

Method

Human objective. The human is modelled as maximising a discounted reward of reaching a goal within width W, minus a weighted torque effort, with neural noise that grows with muscle activity §2.1 eq. 1 eq. 2 eq. 3:

J(\tau(t)) = \mathbb{E}\int_0^\infty \left( e^{-t/\gamma} R(q,g) - \nu \lVert \tau(t) \rVert^2 \right) dt eq. 1

R(q,g) = \begin{cases} 1 & \text{if } |x_{ee}(q) - g| < W \\ 0 & \text{otherwise} \end{cases} eq. 2

M(q)\ddot{q} + D(\dot{q}) + G(q) = \tau\,(I + \epsilon), \qquad \epsilon \sim \mathcal{N}(0, \kappa) eq. 3

Discretisation. The continuous dynamics are linearised and integrated with forward Euler. The state is s = [q; q̇], with mean μ and covariance Σ. Matrices A and B are built from the step length h, the inertia M and the damping D §3.1 eq. 4. The paper uses the linear Gaussian form for the closed-form expectation.

Smooth reward. The binary reward is replaced by a Gaussian. The goal is distributed as N(ḡ, W), and the reward is the probability that the end-effector matches the goal §3.2 eq. 5. Under a Gaussian state, the expected reward has a closed form, with the Jacobian J mapping joint covariance to Cartesian covariance. The derivation is in the appendix titled "Expected reward" §3.2 eq. 6 app. Expected reward:

R(\mu,\Sigma) = \frac{\exp\left(-\tfrac{1}{2}\lVert \mu_x - \bar g \rVert_{(\Sigma_x + W)^{-1}}\right)}{\sqrt{\lvert 2\pi(\Sigma_x + W)\rvert}}, \qquad \mu_x = x_{ee}(\mu_q),\ \Sigma_x = J\Sigma_q J^T eq. 6

Planning problem. Over horizon H, the planner minimises the sum of discounted reward and torque effort, with the initial mean fixed to the current state and the initial covariance zero §3.3 eq. 7 eq. 8 eq. 9:

J(\tau_{0:H}) = \sum_{i=0}^{H} \left( \gamma^{i-t} R(\mu_i, \Sigma_i) + \nu \lVert \tau_i \rVert^2 \right) eq. 7

\tau_{0:H} = \arg\min J(\tau_{0:H}) \quad \text{subject to} \quad \mu_0 = s_0,\ \Sigma_0 = 0,\ \text{dynamics [eq. 4]} eq. 8, eq. 9

The optimiser and its settings are not stated.

# Expected reward of the human state  [§3.2] [eq. 6]
# Input:  mean (2 n_q,), covariance (2 n_q, 2 n_q), goal mean (3,), width W
# Output: scalar reward, a Gaussian density value (not a probability)

def expected_reward(mean, covariance, goal_mean, width):
    # Map the joint state to a Cartesian mean and covariance  [§3.2]
    position_mean = end_effector(mean[:n_q])                  # (3,)
    jacobian = end_effector_jacobian(mean[:n_q])              # (3, n_q)
    position_cov = jacobian @ covariance[:n_q, :n_q] @ jacobian.T  # (3, 3)
    # Add the goal width to the position covariance  [eq. 6]
    total_cov = position_cov + width                          # (3, 3)
    offset = position_mean - goal_mean                        # (3,)
    # Quadratic form with the squared norm; see Pitfalls  [app. Expected reward]
    exponent = -0.5 * offset.T @ inv(total_cov) @ offset
    return exp(exponent) / sqrt(det(2 * pi * total_cov))

# One Euler step of the linear Gaussian human model  [§3.1] [eq. 4]
# Input:  mean (2 n_q,), covariance (2 n_q, 2 n_q), torques (n_q,)
# Output: next mean and covariance

def human_step(mean, covariance, torques, mass, damping, h, kappa):
    # Forward-Euler matrices from the continuous dynamics  [§3.1] [eq. 4]
    A = [[I, h * I], [0, I - h * inv(mass) @ damping]]
    B = [[0], [h * inv(mass)]]
    # Propagate the mean through the linear map  [eq. 4]
    next_mean = A @ mean + B @ torques
    # Propagate covariance and add torque-dependent neural noise  [eq. 4]
    # The paper's noise term is ambiguous; see Pitfalls
    noise = B @ torque_noise_covariance(torques, kappa) @ B.T
    next_covariance = A @ covariance @ A.T + noise
    return next_mean, next_covariance

Multi-component strategy. Reaching is split into an initial ballistic sub-movement and a final corrective one §2.2. The transition point is taken to be where dispersion along a reach is highest, and it depends on distance and goal width §2.2. A Gaussian process regression is fitted to the earlier experimental data to predict the transition position from the distance to the goal, normalised by arm length fig. 2 §2.2. Authority is handed over at that point: in the handover task the robot holds 400 N/m of translational stiffness until the transition, then sets it to 0 §5.2.

Goal estimation. A measured state s^o at time t_o is added as an equality constraint, and the goal ĝ becomes a decision variable with a regulariser on its distance from ḡ §4.3 eq. 10 eq. 11:

\tau_{1:H}, \hat g = \arg\min_{\tau, g} \; J(\tau_{1:H}, \hat g) + \lVert \hat g - \bar g \rVert_{W^{-1}}, \qquad \text{subject to } s_{n_o} = s^o,\ \text{eq. 9} eq. 10, eq. 11

with n_o = floor(t_o / h) §4.3.

Synchronised co-manipulation. The robot starts with zero translational stiffness. The human starts the motion. At t_o = 0.2 s the position and velocity are measured, and the goal-estimation problem is solved once. The resulting Cartesian trajectory is sent as the rest position of an impedance controller with stiffness [0, 0, 400] N/m. The trajectory is executed without re-planning §5.1.

Hyperparameters

Values for the simulated planar arm and the Franka Emika experiments. Items marked "not stated" are absent from the paper.

Results

Author-reported. Setup: simulated planar two-DoF arm §3.4; Franka Emika seven-DoF arm with impedance control §5; one operator, five trials per goal §5.1 §5.2. No baseline controller is run in the experiments. Human-only trajectories are described only qualitatively §5.1.

Paper vs code

The paper links code at a GitLab address under the authors' Fraunhofer group [title block, code footnote]. That host could not be reached from the compilation environment (connection refused), so the code was not read and no commit or license is recorded. GitHub searches for "hri-mpc", the author names and the topic found only unrelated forks and third-party copies, and no repository owned by the authors. No discrepancy check against the code has been done.

Pitfalls

• Sign of the reward term. Equation 1 maximises discounted reward minus effort eq. 1. Equation 7 minimises discounted reward plus effort eq. 7. Minimising the paper's own J as written favours low reward. The intended sign is not stated; an implementer must choose it, and the code could not be checked.
• Discount form. Equation 1 uses e^{-t/γ} with continuous t eq. 1. Equation 7 uses γ^{i-t} over discrete steps eq. 7. How γ = 0.97 maps to a per-step factor, and what t is in eq. 7 (apparently the current step n, not stated), are not given.
• W has two roles. The text describes W as a 3-vector goal radius §2.1. Equation 5 uses |W| and W^{-1}, which only make sense for a covariance matrix eq. 5. The table gives W as a scalar width in metres tab. 1. Which form to implement is not stated.
• Squared norm missing in eq. 6. The text defines ‖x‖²_M = xᵀMx §3.2. Equation 6 and the appendix write the exponent without the square eq. 6 app. Expected reward, but the Gaussian derivation in the appendix needs the quadratic form. The squared form is used in the pseudocode above.
• Reward scale. Equation 5 is a normalised Gaussian density, so R is not bounded by 1 and depends on |W| eq. 5. The effort weight ν = 1e-5 tab. 1 is therefore only meaningful against that scale. Check the magnitude of R across the width sweep (0.005 to 0.04 m) before fixing ν.
• Noise term is ambiguous. The model writes torque noise as τ(I + ε) with ε ~ N(0, κ) eq. 3. The covariance update eq. 4 is written as Bτ(1 + κ)τᵀBᵀ. Read as an outer product of the torque vector, that does not match the element-wise noise model. The element-wise form diag(τ)κdiag(τ) is one reading and is an inference, not a stated formula. Check the code before fixing this.
• Dimension typos. Below eq. 3, M and D are called matrices in R^{n_q}. The planner treats D as a matrix eq. 4 and the table lists D = 0.3I tab. 1.
• "MPC" in name, open loop in use. The planner is described as solved "from a state s_n" §3.3. In the experiments it is solved once and executed open-loop §3.4 §5.1. No receding-horizon re-solve is reported.
• Solve time against horizon. For the Franka case, 50 steps of 0.035 s span 1.75 s of motion tab. 1, while one solve takes 1.3 s tab. 1. With open-loop execution, the robot cannot react to what it observes during that solve.
• Goal estimation needs a start guess. The Discussion states that the approach still needs an initial guess for the human goal §6. How that guess is chosen, and how ‖ĝ − ḡ‖ weighting by W^{-1} is scaled eq. 10, are not stated. Reusing ḡ as both reward goal and prior mean is implied, not stated.
• Transition model inputs. The GP predicts the transition from distance and goal size, normalised to arm length fig. 2. The arm length of the robot, the kernel, and the training split are not stated.
• Impedance switch. Stiffness goes from 400 N/m to 0 at the transition point §5.2. The rotational stiffness value is not stated §5.1. The paper reports no velocity jump fig. 9, but no numeric jump bound is given.
• Payload error. The 1.5 kg payload is not compensated and adds z-axis error §5.1. A deployment needs gravity compensation the paper does not describe.
• Sample size. The validation uses one operator and five trials per goal §5.1 fig. 8 fig. 9. Human-likeness is not tested with users; the paper lists this as future work §6.
• Qualitative validation. Matches to Fitts' law and velocity asymmetry are visual §3.4 fig. 3. No fit statistic is reported.

Lineage

• Builds on: Peternel, Sigaud and Babič, "Unifying speed-accuracy trade-off and cost-benefit trade-off in human reaching movements" [peternel2017unifying]; the paper's own bibliography gives no arXiv id, and that work is not in this wiki. The paper cites it for the binary reward indicator eq. 2 and for combining the two trade-offs §2.1 §3.2.
• Builds on: Peternel and Babič on the target of the initial sub-movement [peternel2019target], which supplies the transition-point data fig. 2.
• Compared against: Dallard et al., "Synchronized human-humanoid motion imitation" [dallard2023synchronized], a minimum-jerk approach. The comparison is qualitative only §1.
• Superseded by: not known from content in this wiki.