Installation and startup: Setup.
Experiment video: Watch the experiments on Bilibili.
This work focuses on dual-arm manipulation in narrow spaces. It combines a trajectory generation algorithm, a distributed nonlinear model predictive control (DNMPC) algorithm, and a bilateral position-force control algorithm with adaptive impedance. Simulations utilize a ball and a box to validate the algorithms. The real-world experiments cover grasping, transporting, placing, and rotating targets with different elastic properties.
Keywords: dual arms, trajectory generation, distributed nonlinear model predictive control, position-force control.
A distributed nonlinear model predictive control (DNMPC) algorithm is proposed to achieve coordinated trajectory tracking and collision avoidance while reducing the control-command cost. By accounting for the heterogeneous kinematics and physical constraints of the dual arms, the algorithm is proposed to ensure kinematic feasibility while maintaining coordinated manipulation.
The bilateral position-force control algorithm generates the desired EE positions.
Parameters are selected by balancing successful task completion, trajectory tracking, internal-force regulation, execution time, and computation time. The index
The sampling period is
In the table, CNMPC denotes centralized nonlinear model predictive control,
Increasing
| Algorithm |
|
Force RMSE (N) | Mean computation time (ms) | Deadline miss (%) | ||
|---|---|---|---|---|---|---|
| CNMPC | 2 | 0.04 | 0.09 | 1.63 | 16.67 | 0.20 |
| CNMPC | 5 | 0.10 | 0.09 | 1.61 | 18.67 | 1.05 |
| CNMPC | 7 | 0.14 | 0.09 | 1.61 | 19.87 | 5.30 |
| CNMPC | 10 | 0.20 | 0.09 | 1.60 | 21.55 | 48.70 |
| DNMPC without state exchange | 2 | 0.04 | 0.09 | 2.14 | 15.37 | 0.00 |
| DNMPC without state exchange | 5 | 0.10 | 0.08 | 2.37 | 16.69 | 0.00 |
| DNMPC without state exchange | 7 | 0.14 | 0.08 | 2.39 | 17.48 | 0.10 |
| DNMPC without state exchange | 10 | 0.20 | 0.08 | 2.39 | 25.49 | 30.20 |
| Ours | 2 | 0.04 | 0.09 | 1.92 | 15.33 | 0.00 |
| Ours | 5 | 0.10 | 0.08 | 1.88 | 16.63 | 0.00 |
| Ours | 7 | 0.14 | 0.08 | 1.88 | 17.48 | 0.00 |
| Ours | 10 | 0.20 | 0.08 | 1.88 | 25.81 | 22.55 |
The parameters are selected according to two principles in this paper. First, the success rate is given the highest priority since the generated trajectories must be feasible. Second, we balance the performance metrics so that improving some metrics does not substantially worsen the others.
The selected weights are:
In practice, we first select initial parameters according to the task requirements. We then tune one parameter while keeping the others fixed, comparing
-
Trajectory length cost,
$w_{\mathrm{p}i}=10$ . Selecting$\times0$ increases the trajectory length cost by 2.45% and the optimization time by 1259.72% in the grasping phase and decreases the success rate by 5% in the transporting phase. Selecting$\times2$ decreases the grasping success rate by 5% and increases the transporting optimization time by 26.82%. -
Joint-velocity cost,
$w_{\mathrm{u}i}=1$ . Selecting$\times0$ increases the joint-velocity cost by 210.98% and 81.09% and decreases the success rates by 5% and 10% in the grasping and transporting phases, respectively. Selecting$\times2$ increases the execution time by 4.14% and 10.36% and the optimization time by 23.43% and 24.27% in the grasping and transporting phases, respectively. -
Other weights. For the distance and orientation constraint weights during transporting,
$w_{\mathrm{p}i,\mathrm{t}}=w_{\mathrm{o}i,\mathrm{t}}=1000$ , the joint-position and joint-velocity constraint weights,$w_{\mathrm{q}i}=w_{\mathrm{v}i}=1000$ , the terminal distance and orientation constraint weights during grasping,$w_{\mathrm{p}i,\mathrm{g}}=w_{\mathrm{o}i,\mathrm{g}}=1000$ , and the time cost weight,$w_{\mathrm{T}i}=1$ , selecting$\times0$ or$\times2$ decreases the success rate or substantially worsens the performance metrics, as shown in bold below.
| Weight setting | Success rate (%) | Trajectory length cost change (%) | Joint-velocity cost change (%) | Execution time change (%) | Optimization time change (%) |
|---|---|---|---|---|---|
Success rates are reported for 20 trajectories in each phase. The other columns report percentage changes relative to the corresponding
| Weight setting | Success rate (%) | Trajectory length cost change (%) | Joint-velocity cost change (%) | Execution time change (%) | Optimization time change (%) |
|---|---|---|---|---|---|
Success rates are reported for 20 trajectories in each phase. The other columns report percentage changes relative to the corresponding
The weight matrices vary across the four manipulation stages: Stages 1 and 2 correspond to trajectory tracking and completion of grasping, and Stages 3 and 4 correspond to trajectory tracking and completion of transporting. Here,
| Phase | Stage | |||||||
|---|---|---|---|---|---|---|---|---|
| Grasping | 1 | |||||||
| Grasping | 2 | |||||||
| Transporting | 3 | |||||||
| Transporting | 4 |
The Stage 3 comparison uses 20 experiments per setting and changes one weight at a time to zero or twice its selected value, holding the others fixed. The terminal-position weight remains
-
Relative-position error weight
$W_{\mathrm{t}i}$ . Setting the weight to zero decreases the success rate from 100% to 95%. Increasing the weight from$\times1$ to$\times2$ increases the force RMSE by 2.92% and the total DNMPC objective$E_i$ by 1.81%. Therefore,$W_{\mathrm{t}i}=40I_3$ is selected. -
Orientation coordination weight
$W_{\mathrm{o}i}$ . Setting the weight to zero reduces the success rate from 100% to 80%. Increasing the weight from$\times1$ to$\times2$ increases the relative-position RMSE by 23.08%, the force RMSE by 30.19%, and$E_i$ by 4.01% and reduces the success rate from 100% to 70%. Therefore,$W_{\mathrm{o}i}=2I_3$ is selected. -
Control-command cost weight
$W_{\mathrm{u}i}$ . Setting the weight to zero increases the control-command cost by 8.70%. Increasing the weight from$\times1$ to$\times2$ increases the joint RMSE and joint-velocity RMSE by 30.40% and 14.86%, respectively, and increases$E_i$ by 7.57%. Therefore,$W_{\mathrm{u}i}=0.02I_6$ is selected. -
Joint-configuration tracking error weight
$W_{\mathrm{q}i}$ . Setting the weight to zero reduces the success rate from 100% to 0% and increases the joint RMSE by 101.52%. Increasing the weight from$\times1$ to$\times2$ increases the force RMSE by 29.61% and reduces the success rate from 100% to 85%. Therefore,$W_{\mathrm{q}i}=7.5I_6$ is selected. -
Joint-velocity tracking error weight
$W_{\mathrm{v}i}$ . Setting the weight to zero reduces the success rate from 100% to 0% and increases the joint-velocity RMSE by 771.54%. Increasing the weight from$\times1$ to$\times2$ increases the force RMSE by 19.78% and reduces the success rate from 100% to 90%. Therefore,$W_{\mathrm{v}i}=3I_6$ is selected. -
Weight
$W_{\mathrm{F}i}$ for the position-tracking error with respect to the desired EE position generated by the bilateral position-force control algorithm. Setting the weight to zero increases the force RMSE by 650.64% and reduces the success rate from 100% to 95%. Increasing the weight from$\times1$ to$\times2$ increases the joint-velocity RMSE by 71.29%. Therefore,$W_{\mathrm{F}i}=100I_3$ is selected.
In summary, the
| Weight setting | Success rate (%) | Joint RMSE change (%) | Relative-position RMSE change (%) | Orientation RMSE change (%) | Joint-velocity RMSE change (%) | Force RMSE change (%) | Command cost change (%) |
|
|---|---|---|---|---|---|---|---|---|
Success rates are absolute values, whereas all other entries denote changes relative to the corresponding
| Parameter | Selected value | Role in the comparison |
|---|---|---|
| 200 | Compared with 0 and 400 | |
| 10 | Compared with 0 and 20 | |
| 500 | Held fixed |
The adaptive damping weight
For the adaptive damping weight
For the adaptive stiffness weight
| Parameter setting | Success rate (%) | Relative-position RMSE change (%) | Force RMSE change (%) | Joint-velocity RMSE change (%) | Control-command cost change (%) |
|---|---|---|---|---|---|
The baseline settings are
We first adjust the target and the force sensors so that the normal forces are along the line joining the force sensors' centers and perpendicular to the gravitational force, and stably grasp the target. We then gradually reduce the grasping force until the motion capture system detects slip onset. The force data and target-center positions are recorded simultaneously to determine the slip-onset force. This procedure is repeated five times for each target. The mean slip-onset force is multiplied by 1.5 to set the desired internal force.
In the table below, Center Position (m) is the distance in meters from the left contact point to the position that divides the target into two parts of equal mass.
| Target | Mass (kg) | Grasping Size (m) | Center Position (m) | Mean Slip-Onset Force (N) | Desired Internal Force |
|---|---|---|---|---|---|
| Ball | 0.19 | 0.18 | 0.09 | 10.29 | 15.43 |
| Box | 0.38 | 0.25 | 0.13 | 14.80 | 22.20 |
| Basketball | 0.60 | 0.25 | 0.125 | 21.60 | 32.40 |
| L-shaped wire mesh | 0.15 | 0.29 | 0.21 | 7.57 | 11.35 |
| Plush toy | 0.12 | 0.19 | 0.11 | 11.16 | 16.74 |
| Foam frame | 0.04 | 0.19 | 0.10 | 2.73 | 4.10 |
| Clothes-filled bag | 1.30 | 0.23 | 0.12 | 26.14 | 39.21 |
| Parameter | Grasping | Transporting |
|---|---|---|
| Safety margin (m) | ||
| Allowable orientation to the other arm, left arm (rad) | ||
| Allowable orientation to the other arm, right arm (rad) | ||
| Allowable orientation to the target, left arm (rad) | ||
| Allowable orientation to the target, right arm (rad) |
| Parameter | Grasping | Transporting |
|---|---|---|
| Safety margin (m) | ||
| Allowable orientation to the other arm, left arm (rad) | ||
| Allowable orientation to the other arm, right arm (rad) | ||
| Allowable orientation to the target, left arm (rad) | ||
| Allowable orientation to the target, right arm (rad) |
Each arm is controlled by its own Intel NUC11 computer with an Intel Core i7-1165G7 CPU at 2.80 GHz and 16 GB RAM, running Ubuntu 20.04 and ROS1.
| With AIC algorithm | Without AIC algorithm |
|---|---|
![]() |
![]() |
| With AIC algorithm | Without AIC algorithm |
|---|---|
![]() |
![]() |
| With AIC algorithm | Without AIC algorithm |
|---|---|
![]() |
![]() |
| With AIC algorithm | Without AIC algorithm |
|---|---|
![]() |
![]() |
| With AIC algorithm | Without AIC algorithm |
|---|---|
![]() |
![]() |
| With AIC algorithm | Without AIC algorithm |
|---|---|
![]() |
![]() |
| With AIC algorithm | Without AIC algorithm |
|---|---|
![]() |
![]() |
Four experiments compare left- and right-controller faults with and without predicted state exchange. The fault lasts 5 s and reverses the sign of one component in one transmitted EE position or orientation state. During the fault, both reference trajectories are held at their values at the fault start, and zero control commands keep the arm with the faulty controller at its current joint configuration.
With predicted state exchange, the controllers exchange current and predicted EE positions and orientations. Without predicted state exchange, they exchange only the current EE positions and orientations. The experiments show that the dual arms maintain the grasp with predicted state exchange under the considered fault conditions.
| With predicted state exchange | Without predicted state exchange |
|---|---|
![]() |
![]() |
| With predicted state exchange | Without predicted state exchange |
|---|---|
![]() |
![]() |
| With AIC algorithm | Without AIC algorithm |
|---|---|
![]() |
![]() |
| Algorithm | EE | Desired Force (N) | Maximum (N) | Minimum (N) | Mean (N) | RMSE (N) |
|---|---|---|---|---|---|---|
| With AIC | L | 15.43 | 27.66 | 12.51 | 16.88 | 4.30 |
| With AIC | R | 15.43 | 27.79 | 15.10 | 17.89 | 4.24 |
| Without AIC | L | 15.43 | 37.89 | 18.56 | 30.45 | 15.89 |
| Without AIC | R | 15.43 | 34.67 | 22.16 | 28.27 | 13.29 |
| With AIC algorithm | Without AIC algorithm |
|---|---|
![]() |
![]() |
| Algorithm | EE | Desired Force (N) | Maximum (N) | Minimum (N) | Mean (N) | RMSE (N) |
|---|---|---|---|---|---|---|
| With AIC | L | 22.20 | 25.89 | 15.69 | 20.87 | 2.99 |
| With AIC | R | 22.20 | 24.93 | 15.45 | 20.81 | 2.92 |
| Without AIC | L | 22.20 | 45.00 | 24.31 | 35.91 | 14.84 |
| Without AIC | R | 22.20 | 41.85 | 23.88 | 34.33 | 13.21 |
The development environment is Ubuntu 20.04 LTS with ROS 1 Noetic and Python 3.8. The ROS workspace is CM_ws. Run the commands below in Linux terminals.
- Operating system: Ubuntu 20.04 LTS (64-bit).
- ROS: ROS 1 Noetic with catkin; this workspace is not a ROS 2 package.
- Simulation: Gazebo Classic with ROS control plugins.
- Python: Python 3.8, NumPy, PyYAML and CasADi 3.6.3 with Ipopt.
Additional dependencies not covered by the commands above:
-
Planning: Pinocchio and hpp-fcl development libraries, including their CMake package configurations. FCL and hpp-fcl are separate dependencies; both are required by this workspace.
-
Real-world experiments: the UR ROS driver and robot-side External Control, as described below.
From the repository root:
source /opt/ros/noetic/setup.bash
cd CM_ws
export CM_WORKSPACE="$PWD"
catkin_make
source devel/setup.bashRun the following in each new terminal, from CM_ws:
source /opt/ros/noetic/setup.bash
source devel/setup.bashThe current source layout is:
CM_ws/src/
simulation/
cm_ball/
cm_box/
real_world_experiment/
cm_dnmpc/
cm_trajectory_generation/
asset/
robotiq_ft_sensor/
ur3_description/
ur5_description/
Run one task at a time:
# Ball
roslaunch cm_ball sim.launch# Box
roslaunch cm_box sim.launchEach launch starts Gazebo and both controllers. Gazebo starts paused; press Play after initialization.
Single arm launch files are in cm_dnmpc/launch. They depend on the existing ur_robot_driver, ur_description and ur_client_library installation, including the file ur5e_controllers.yaml and ur3_controllers.yaml driver configurations.
# UR5e: computer 192.168.1.80
roslaunch cm_dnmpc ur5e_urdriver.launch# UR3: computer 192.168.2.90
roslaunch cm_dnmpc ur3_urdriver.launchOn the each arm's computer, the same files are also installed under ur_robot_driver/launch, so roslaunch ur_robot_driver ur5e_urdriver.launch and roslaunch ur_robot_driver ur3_urdriver.launch are available. Use one entry per arm, not both package variants.
| Arm | Robot IP | Computer / reverse IP | Reverse port | Script sender | Trajectory | Script command |
|---|---|---|---|---|---|---|
| UR5e / left | 192.168.1.100 | 192.168.1.80 | 50001 | 50002 | 50003 | 50004 |
| UR3 / right | 192.168.2.20 | 192.168.2.90 | 50005 | 50006 | 50007 | 50008 |
Grant read/write permission to the actual local serial device before launching its sensor:
# UR5e
sudo chmod a+rw /dev/ttyUSB1
roslaunch robotiq_ft_sensor ur5e_ft300.launch serial_id:=ttyUSB1# UR3
sudo chmod a+rw /dev/ttyUSB0
roslaunch robotiq_ft_sensor ur3_ft300.launch serial_id:=ttyUSB0If the UR5e sensor is ttyUSB0 on its computer, change both occurrences to ttyUSB0. Serial devices need read/write permission, not executable permission. Keep sensors unloaded and stationary during controller zero calibration.
The real controller accepts externally computed target-center acceleration and projects F_con,i + m_i*g - m_i*a_target onto the line joining the EEs, where F_con,i is the measured contact force. The projected vector is the internal force F_i, and its magnitude is used as the internal-force feedback. The mass assigned to each arm is half the object.mass_kg value in the object profile. Tool gravity compensation remains separate.
The acceleration input is geometry_msgs/AccelStamped: supply the target-center acceleration in accel.linear, in m/s² and in the same common frame as the contact forces. Obtain this acceleration by differencing the target-center positions outside the controller. This input is kinematic acceleration; gravity is added separately by the force formula. No motion-capture subscription or position differentiation is performed by the real controller.
The default topic is /target/acceleration. Set --target-acceleration-topic YOUR_ACCELERATION_TOPIC or the private parameter ~target_acceleration_topic to the actual input. For real.launch, left.launch, or right.launch, use target_acceleration_topic:=YOUR_ACCELERATION_TOPIC. --target-acceleration-frame FRAME_ID optionally checks the frame name; it does not transform coordinates. The input must have increasing timestamps synchronized with the controller's ROS clock. --target-acceleration-timeout defaults to 0.2 s; missing or stale input is not replaced by zero acceleration. Logs include the supplied acceleration, its timestamp and both internal-force vectors. The existing simulation force model is unchanged.









































































