Control Strategies for Dual Arm Co-Manipulation of Flexible Objects in Industrial Environments

The introduction of collaborative robots had a great impact in the development of robotic solutions for cooperative tasks typically performed by humans, specially in industrial environments where robots can act as assistants of operators. Even so, the coordinated manipulation of large and deformable parts between dual arm robots and humans rises many technical challenges, ranging from the coordination of both robotic arms to the detection of the forces applied by the operator. This paper presents a novel control architecture for the execution of trajectory driven collaborative tasks, combining impedance control and trajectory coordination in the control loop. The obtained results demonstrate the validity of the implemented control architecture as well as its suitability for the implementation of collaborative cyber-physical systems.


I. INTRODUCTION
The introduction of collaborative robots had a great impact in the development of robotic solutions for cooperative tasks. Industrial environments are an interesting space for the use of collaborative robots as these robots could act as assistants of operators to help them in their daily tasks.
Even though, the implementation of cooperative operations between humans and robots rises many challenges, specially with dual arm robots. Within all these challenges, one key aspect which is shared among almost all of them is the exchange of implicit information between both actors. For example, two humans are able to transport and place large parts without any visual information of their partner, using only the feedback of the forces sensed during the manipulation to adapt their trajectories and fulfil the task. This adaptation is even more important when deformable parts are manipulated, increasing the number of unexpected events during the manipulation. This paper presents a novel control architecture for dual arm co-manipulation of flexible parts. The proposed approach is based on three key elements: 1) Human driven co-manipulation, 2) adaptation of trajectories to unexpected events and 3) coordination of dual arm robots. The presented architecture tackles these three elements, defining a new control scheme for dual arm co-manipulation tasks. The implementation and testing of the control architecture shows its suitability for cooperative industrial tasks, acting as core element in the development of a collaborative cyber-physical systems.
The paper is organized as follows: Section II provides information about related work. Section III presents the proposed architecture in depth. Details about the implementation of the architecture are given in section IV. Section V presents the carried out experiments. Finally, section VI gives information about the conclusions and future work.

II. RELATED WORK
The collaboration between humans and robotic manipulators is a recurrent research topic with multiple scenarios and approaches proposed. From classical scenarios with standard robotic manipulators to the use of humanoid robots, many works of human-robot co-manipulation can be found in the literature.
Lichiardopol et al. [1] propose a control scheme for humanrobot co-manipulation with a single robot, where the system estimates an unknown and time-varying mass as well as the force applied by the operator. Dimeas and Aspragathos [2], [3] propose a method to detect unstable behavior and stabilize the robot with online adaptation of the admittance control gains, adding reinforcement learning to estimate parameters for effective cooperation. Peternel et al. [4] propose an approach for co-manipulation tasks such as sawing or bolt screwing through a human-in-the-loop framework which integrates online information about the human motor function and manipulability properties.
Jlassi et al. [5] introduce a modified impedance control method for heavy load co-manipulation where an event controlled online trajectory generator is included to translate the human operator intentions into ideal trajectories. Following the topic of trajectory generation, Raiola et al. [6] propose a framework to design virtual guides through demonstration using Gaussian mixture models.
Focusing on the medical sector, Baumeyer et al.
[7] present a robotic system for patient positioning, co-manipulated with a haptic device and using admittance control.
Based on humanoid robots, Otani et al. [8] present a multi-robot quadratic program controller, which solves human dynamics reconstruction and optimal robot control in a single optimization problem.

III. PROPOSED APPROACH
As stated previously, the aim of the proposed approach is to provide a dual arm robot able to assist human operators in manipulation tasks with large and deformable parts. The type of task and managed objects, as well as the way these large parts are transported by humans nowadays, introduce several requirements: • During large part transport by humans, both actors agree (implicit or explicitly) on an approximate trajectory to execute during the transportation phase. Following this premise, robots will manage a nominal trajectory which can be manually defined by users or generated automatically (e.g. using artificial vision). • When humans transport large parts along a previously agreed path, both actors are able to deform this trajectory in a coordinated way to reach the desired goal, deviations that are even larger when dealing with deformable objects. The implementation of impedance control [9], [10] is proposed to mimic this behaviour. • As robots act as assistants of the human, robots will only advance in the trajectory when the operator moves the part in the defined path. Therefore, humans always have the master role in the co-manipulation task. • Both robots need to move with a degree of coordination although this coordination will be looser than in rigid bodies, as deformable parts may require the adaptation of both robots. Taking into account these features and restrictions, next section describes the key elements of the Trajectory Driven Guidance with Impedance Control.

A. Trajectory Driven Guidance with Impedance Control
This approach allows the addition of a nominal trajectory to the impedance control while maintaining the coordination between the two robots.
The main idea is to follow a given trajectory as a human guides the robots: the robot will show no resistance along the trajectory while the resistance will increase in the directions orthogonal to the nominal path. Additionally, impedance control is added to allow deformations on the path. It will provide some freedom to the user as long as it guides the robot near the nominal trajectory. The control algorithm uses, besides the impedance control parameters, a set of trajectory points which will be linearly interpolated. In each step, the current robot pose will be projected to the nominal trajectory to define the new trajectory pose, as shown in Fig. 1. The algorithm only allows to advance forward along the trajectory, maintaining the position if the projection vector points backwards as depicted in Fig. 2.
To this end, a two step control scheme is proposed for each robot. In a first step the Guidance module calculates the next trajectory pose X d based on the nominal trajectory, current robot pose and percentage of trajectory covered by both arms. In a second step, the Impedance control module modifies this pose in order to obtain a compliant behaviour,  calculating reference pose X r . Fig. 3 shows the architecture of the proposed approach for the dual arm co-manipulation of flexible objects. Next lines provide further information about the Guidance module and Impedance control module.

B. Guidance Module
The initial layer of the control algorithm is based on the Trajectory Driven Guidance with Impedance Control described previously and calculates the setpoint sent to the Impedance control module. Specifically, these are the steps followed by the control algorithm to define the next setpoint (theoretical robot pose) in each loop: • Project the current robot pose X in the vector where X i d is the current setpoint and B is the end of the current segment of the trajectory.
• Then the correction factor µ is calculated using values α, β and λ; where α is the percentage of the trajectory coveved by the robot, β is the percentage of the trajectory of the other robot and parameter λ allows to tune this correction factor, adjusting the increase and decrease rate. Additionally, the direction of the projection vector − → P is checked; if the vector points backwards the correction factor µ is set to 0 to avoid reverse movements.
• At this step, the corrected advance vector − → P c is calculated using the projection vector − → P and correction factor µ. It allows reducing the advance when the robot's trajectory coverage is above the other robot's, and increasing this advance otherwise.
while quaternion Q i+1 is interpolated between the rotations of poses A and B using the spherical linear interpolation [11] as • Finally the desired setpoint X d is composed using translation vector This new pose X d is sent to the Impedance control module to be used as setpoint.

C. Impedance Control Module
In a second step a compliant Cartesian behaviour is added to each robotic arm implementing Cartesian impedance control [12], [13]. The Impedance control module allows to establish a mass-damper-spring relationship between the Cartesian position ∆x and the Cartesian force F where M , D, and K represent the virtual inertia, damping and stiffness of the system.
To calculate the reference pose X r , based on the previously calculated setpoint X d and the sensed force vector F : where ∆F represents the difference between the desired contact force and the actual one.
This pose X r is sent to the robot for the execution of the dual arm co-manipulation trajectory.

IV. IMPLEMENTATION
The proposed control architecture has been implemented using a setup of two Kuka LBR iiwa robots with a payload of 7kg for each arm, besides an additional PC connected to both robot controllers. The robots are equiped with automatic tool exchangers and vacuum cups to allow grasping different types of large objects and parts. An external IO module is also available to manage the tool exchangers as well as the suction of the vacuum cups. Fig. 4 shows the setup of the robots. From the software point of view, all the calculi of the Guidance module and Impedance control module have been implemented in Java in the robot controllers. The Impedance control module makes use of Kuka Sunrise's Smart Servo library to close the control loop.
Additionally, the external PC acts as bridge between both robots by mean of a custom ROS node written in C++ which manages the connections and exchanges the trajectory information.

V. EXPERIMENTAL RESULTS
To test the suitability of the proposed control architecture, a set of experiments has been defined. The main goal of the experiments is to track the information about the trajectories and the coordination of both arms during the execution of two different co-manipulation tasks. Specifically, the previously presented dual arm robotic system has been used to transport a flexible 2m long carbon fiber part between a shelf placed in front of the robots and a pair of fixtures in the workbench. The task has been performed in both directions (shelf to fixtures and fixtures to shelf ) and by different people guiding the maneuver, as shown in Fig. 5.
Besides, multiple stiffness and λ values have been used during the test. On one hand, two stiffness configurations have been set; a high stiffnes configuration (k = 250) which allows a slight deviation from the nominal path and a low stiffness (k = 50) which allows to manage the part in a loose way. On the other hand, three different λ values have been tested (0, 100 and 500). A value of λ = 0 disables the coordination of the trajectories while the greater the value, the tighter the coordination. Table I summarizes the results obtained during the experiments. The first two columns indicate the stiffness and λ values. The next two columns provide information about the difference of the percentage of the trajectory covered by the two robots, specifically the maximum and mean values; these values provide information about the coordination level betweeen both arms. Finally, the last columns contain information about the deviation of the robots from the nominal trajectory, described by the maximum and mean deviation values.
The results show that in the maneuvers executed with low stiffness values, the trajectories performed by the operators deviate more from the nominal path than the ones performed with high stiffness values. Specifically, low stiffness values allow to modify the nominal path around 45mm on average, reaching peaks with deviations over 100mm, twice the values obtained with a high stiffness configuration. Additionally, the increment of value λ lowers the difference of trajectory percentages, increasing the coordination of both robots during the maneuver. It is specially noticeable in the tests with low stiffness values where the operators are able to modify the nominal path easily, lowering the maximum difference from 13% to 9% and the mean difference from 3% to 0.73%.
The operators pointed out that the use of low stiffness values make it easier to fulfil the task as the user can adjust the placement of the part without much effort, avoiding the rigidity of the robotic system with high stiffnes values. It provides a better user experience as the system is more responsive to operators' adjustments to unexpected events related with the flexibility of the part. Besides, the coordination obtained with high λ values also help during the task, keeping the system stable when part behaves in a flexible way.
For a closer look on the obtained results, Fig. 6 contains several plots with information about the executions. Figures  6 (a), (b) and (c) show the percentage of trajectory covered by the robots along time, as well as the difference between the percentages (uncoordination) and the deviation from the nominal path with high stiffness parameters and and λ = 0. values and λ = 500. The figures show that the increment of parameter λ helps to increase the coordination even with low stiffness values where the user is able to modify the nominal trajectory up to 100mm. It provides adaptability to operators while maintaining coordination when the flexibitily of the parts intervene during the maneuver.

VI. CONCLUSIONS
This paper presents a control architecture for dual arm co-manipulation tasks which acts as central element of the developed collaborative cyber-physical system. The architecture allows transporting large parts along a nominal trajectory with two robotic arms, using the forces exerted by a human operator to drive the execution and allowing the deformation of the nominal trajectory to adapt it to unexpected events. This last feature is specially interesting for the co-manipulation of flexible parts, where the deformation of the grasped objects make it necesary to adapt the robot trajectory constantly.
The proposed architecture has been implemented and tested in a setup based on two Kuka LBR iiwa robots equiped with vacuum cups to grasp large deformable objects. The obtained results show the suitability of the proposed control architecture, allowing the co-manipulation of 2m long carbon fiber parts to transport them between shelves and fixtures.
Even so, several aspects of the architecture can be developed and improved as future work. On one hand, impedance control parameters could be modified in execution time based on the deformation of the object and human behaviour in order to offer a human-like manipulation experience. On the other hand, the development of mechanisms to provide robot feedback such as percentage of the trajectory covered or information about the deformation of the trajectory could be an interesting addition as humans exchange much information during cooperative tasks besides the applied forces.
Additionally, the Impedance control module could be implemented in an external PC to allow its use in other setups with different robots, possibly equiped with external force-torque sensor. By leveraging the ros control framework, the proposed control law could be implemented in the form of a generic controller accessing each robot's joint positions, as well as the force-torque sensor measurements. This controller would, in turn, send joint position, velocity or torque commands to each robot's joints. By using ros control, the access to the robot joints and force-torque sensors would be executed through abstract, named, hardware interfaces. Therefore, it could be envisioned to use the exact same controller (albeit with possibly differently tuned parameters) to control any combination of robots, provided they offer a ROS driver compatible with the ros control framework.