Multi-Arm Coordination — 2-DOF QUANSER

The Challenge

Coordinating two independent robotic arms to manipulate a shared object requires real-time synchronisation of force and position feedback. One arm acting purely on position commands would cause the object to slip or deform if the other arm exerted unexpected forces. The system needed a control architecture where one arm "felt" the interaction forces and the other precisely tracked the desired trajectory.

Architecture & System Design

Two robot arms coordinate to manipulate shared object: master arm controls trajectory, slave arm adjusts force/compliance based on real-time feedback. Hybrid control law balances position accuracy with force regulation.

The QUANSER 2-DOF arms were interfaced through QUARC real-time control software running MATLAB/Simulink models. The master arm was configured in position control mode, providing the desired trajectory. The slave arm ran an impedance controller that regulated the contact force, adjusting its compliance based on real-time force/torque feedback. A hybrid force-position control law was derived analytically and validated in simulation before hardware deployment.

Code Walkthrough

3-step walk-through of the production implementation — file paths and intent shown above each block.

1. 01

   Step 1 of 3

   Master arm pose acquisition via QUARC encoder interface

   control/master_pose.m

   The QUANSER SRV02 reports raw encoder counts; everything else in the control loop needs radians and rad/s. A persistent variable accumulates the previous angle so numeric differentiation gives velocity without a separate sensor. A low-pass filter smooths the derivative because encoder quantisation noise amplified by 1/dt produces spikes that saturate the impedance controller at 1 kHz.

   matlabCopy

   function [q_master, dq_master] = read_master_pose(enc_ch1, enc_ch2, dt)
   % Read 2-DOF QUANSER SRV02 encoder channels and return joint angles + velocities.
   % Called at 1 kHz from the QUARC real-time Simulink loop.
   
   COUNTS_PER_REV  = 4096;          % 4x decoding enabled in QUARC config
   LOW_PASS_ALPHA  = 0.15;          % velocity filter bandwidth ~24 Hz
   
   % Encoder counts → radians, zero at horizontal reference pose
   theta1 = enc_ch1 * (2 * pi / COUNTS_PER_REV) - THETA1_OFFSET;
   theta2 = enc_ch2 * (2 * pi / COUNTS_PER_REV) - THETA2_OFFSET;
   q_master = [theta1; theta2];
   
   % Numeric differentiation + first-order low-pass (persistent state)
   persistent q_prev dq_prev;
   if isempty(q_prev)
       q_prev  = q_master;
       dq_prev = zeros(2,1);
   end
   
   dq_raw    = (q_master - q_prev) / dt;
   dq_master = LOW_PASS_ALPHA * dq_raw + (1 - LOW_PASS_ALPHA) * dq_prev;
   
   q_prev  = q_master;
   dq_prev = dq_master;
   end

   Takeaway

   Low-pass the velocity estimate, not the position — filtering position introduces phase lag that destabilises the 1 kHz control loop; filtering velocity only limits the spike without shifting the steady-state.

2. 02

   Step 2 of 3

   Impedance controller — slave arm compliance law

   control/impedance_controller.m

   Pure position control on the slave arm would rigidly track the master trajectory and crush any object that pushes back. The impedance controller adds a virtual spring-damper between the desired and actual pose, so the arm yields under unexpected contact force. The Jacobian transpose maps the Cartesian force law back into joint torques the QUARC amplifier can execute.

   matlabCopy

   function tau = impedance_controller(q, dq, F_ext, q_d, dq_d)
   % Impedance control law for 2-DOF QUANSER slave arm.
   % Regulates contact force while tracking master trajectory.
   
   Md = diag([1.2, 1.2]);    % Desired inertia  [kg]
   Bd = diag([8.0, 8.0]);    % Desired damping  [N·s/m]
   Kd = diag([40.0, 40.0]);  % Desired stiffness [N/m]
   
   % Position and velocity errors in joint space
   e   = q_d - q;
   de  = dq_d - dq;
   
   % Impedance law: virtual force from desired impedance + external reaction
   % Md·ddq + Bd·dq_err + Kd·q_err = F_ext + F_cmd
   F_cmd = Md * (Kd * e + Bd * de) + F_ext;
   
   % Map to joint torques via geometric Jacobian transpose
   J   = compute_jacobian(q);      % 2×2 for planar 2-DOF arm
   tau = J' * F_cmd;
   
   % Saturate to protect actuators (±3 Nm per joint)
   tau = min(max(tau, -TAU_MAX), TAU_MAX);
   end

   Takeaway

   Kd (stiffness) sets how tightly the slave tracks position; Bd (damping) prevents oscillation at contact. Tuning these two diagonals defines the feel of the interaction without changing the control law structure.

3. 03

   Step 3 of 3

   Force feedback reflection to master arm

   control/force_feedback.m

   Without haptic feedback the operator driving the master arm has no sense of how hard the slave is pressing against the object — they can easily command destructive forces. Reflecting a scaled fraction of the contact force back as a resistive torque on the master arm closes the kinesthetic loop, giving the operator tactile sense of the interaction.

   matlabCopy

   function apply_force_feedback(q_master, F_slave_contact)
   % Reflect slave contact force to master arm as haptic resistance.
   % Full reflection (gain=1) is too stiff for an operator; 0.4 is the
   % empirically stable limit for the QUANSER SRV02 actuators.
   
   REFLECTION_GAIN  = 0.4;
   MAX_FEEDBACK_NM  = 2.0;   % Operator safety saturation [N·m]
   
   J_master    = compute_jacobian(q_master);       % Jacobian at current master pose
   tau_reflect = REFLECTION_GAIN * (J_master' * F_slave_contact);
   
   % Saturate to protect operator and actuator
   tau_reflect = min(max(tau_reflect, -MAX_FEEDBACK_NM), MAX_FEEDBACK_NM);
   
   % Write to QUARC DAC — QUANSER: ±10 V → ±tau_max via motor constant K_t
   V_cmd = tau_reflect / (K_T * AMP_GAIN);
   write_analog_channels([V_cmd(1), V_cmd(2)]);
   end

   Takeaway

   The reflection gain is the only degree of freedom between 'operator feels nothing' and 'actuator slams back' — saturate before the DAC write, not after, so the voltage rail never clips the signal asymmetrically.

Results

The master-slave system successfully demonstrated coordinated object manipulation with stable force regulation. The impedance controller maintained contact forces within ±0.2 N of the setpoint during dynamic manipulation tasks, enabling safe and compliant interaction with the shared object.

Gallery & Demos

Click any image or video to expand · ← → keys navigate