Robotic manipulation can involves multiple manipulators to complete a task. In those cases, the complexity of performing the task in a coordinated manner increases, requiring coordinated planning while avoiding collisions between robots and environmental elements. For these challenges, we propose a robotic arm control algorithm based on Learning from Demonstration to independently learn the tasks of each arm, followed by a graph-based communication method using Gaussian Belief Propagation. Our method enables the resolution of decoupled dual-arm tasks learned independently without requiring coordinated planning. The algorithm generates smooth, collision-free solutions between arms and environmental obstacles while ensuring efficient movements without the need for constant replanning. Its efficiency has been validated through experiments and comparisons against another multi-robot control method in simulation using PyBullet with two opposing IIWA robots, as well as a mobile robot with two UR3 arms, which has also been used for real-world testing.
Our algorithm is designed to coordinate dual-arm tasks in decoupled scenarios. It consists of two main modules: a Learning from Demonstration (LfD) module that enables each robotic arm to independently learn its task using kinesthetic demonstrations and a Task-Parameterized Gaussian Mixture Model (TP-GMM), and a collaborative control module based on Gaussian Belief Propagation (GBP) that dynamically synchronizes the end-effectors. This second module communicates between the two arms to ensure safe, collision-free movements while adapting in real time to obstacles and dynamic changes in the environment. The approach has been validated through extensive simulation and real-world experiments, demonstrating improvements in execution time, path efficiency, and motion smoothness compared to state-of-the-art methods.
The method presented in this work independently learns to solve a task for each arm. These tasks are then coordinated through the application of a new method based on Gaussian Belief Propagation, which enables coordination while simultaneously avoiding obstacles in the environment by using the learned paths.
Our algorithm is composed of two main modules: the learning module and the coordination module using Gaussian Belief Propagation (GBP). In the learning phase, each arm independently learns its task through kinesthetic demonstrations. During these demonstrations, the positions and orientations of the end-effector are recorded in a task-related reference frame. Then, using a task-parameterized Gaussian Mixture Model (TP-GMM), the invariant structure of the task is extracted. To optimize the model’s accuracy, synthetic data is iteratively generated and integrated only if it improves the solution. In this way, the algorithm can learn a representative trajectory from a limited set of demonstrations. Once each arm has learned its task, GBP is used to coordinate their movements in real time. In this module, each state of the end-effector is represented as a variable in a factor graph. The factors model the dynamic constraints, the presence of obstacles, and the interactions between the two arms. Through an iterative message-passing process, GBP enables the arms to share information about their positions and velocities, adjusting their trajectories to avoid collisions and ensure smooth, synchronized execution.
Below, different examples of the algorithm's functionality in simulated environments are presented. These examples demonstrate the algorithm's efficiency with two opposing IIWA robots. In each case, a critical scenario is presented—these are situations where the robot's movements intersect with each other or cross obstacles in the environment. In all cases, the algorithm successfully resolves the movements efficiently without colliding with environmental elements or the other arm.
Results of the different experiments conducted with the IIWA robots in simulation in terms of movement smoothness, analyzing velocity and position.
Example 1: Opposite movements without obstacles.
Example 2: Opposite movements with 1 obstacles.
Example 3: Opposite movements with 5 obstacles (narrow passage).
Example 4: Obstacle in final point.
The algorithm has also been used for tasks in real environments using the ADAM robot, belonging to the Mobile Robotics group at RoboticsLab, at the Carlos III University of Madrid. In these tests, two real task experiments were conducted where coordination between the two robotic arms is crucial: a cube stacking task and a shelf organization task. In this case, each arm uses a trajectory learned through Learning from Demonstration completely independently, without considering the other arm at any moment. The developed algorithm will handle the necessary coordination. The following two videos demonstrate the efficiency of our algorithm on the real platform, performing complex tasks smoothly and always achieving a satisfactory result.
Cube stacking task.
Shelf organization task.
