Pick-and-place in dynamic environments with a mobile dual-arm robot equipped with distributed distance sensors

—Mobile bimanual manipulation in a dynamic and uncertain environment requires the continuous and fast adjustment of the robot motion for the satisfaction of the constraints imposed by the task, the robot itself and the environment. We formulate the pick-and-place task as a sequence of mobile manipulation tasks with a combination of relative, global and local targets. Distributed distance sensors on the robot are utilized to sense the surroundings and facilitate collision avoidance with dynamic and static obstacles. We propose an approach to kinematically control the robot by solving a priority constrained optimization problem online. Experimental results on the YuMi bimanual robot mounted on the Ridgeback mobile platform validate the performance of the proposed approach.


I. INTRODUCTION
Dual-arm manipulation and manipulators have been presented in detail in several review papers [1], [2]. The recent developments in the areas of humanoid robots and the work on learning by imitation has motivated the construction and use of dual-arm setups. As bimanual robots can mimic human behaviors, act and manipulate objects in ways similar to humans, human bimanual skills can be transferred to them. Consequently, the robot can work interchangeably and in collaboration with humans in the same task. Furthermore, mobile manipulation increases in popularity due to advancements in the control approaches and performance, the reduced costs and the need for mobility in robotic applications [3], [4]. Although, multiarm and mobile robots increase the complexity of the system, they offer multiple end-effectors to interact with the environment and the mobility to work on a broader workspace executing more complex tasks. Their intrinsic redundancy can be further exploited to satisfy constraints that ensure the safety of the robot and its environment, such as avoiding collisions and respecting its joint limits, as well as satisfying additional objectives. The majority of the works on dual-arm coordinated motion have focused on the cooperative task space representation via absolute and relative velocity variables ignoring environmental and self constraints that are present in such problems in practice [5], [6], [7].
Distance sensors provide a solution of low price and computational cost with fewer occlusion problems as opposed to cameras for obstacle detection. The majority of research on collision avoidance with distance sensors focuses on repulsive motions using potential fields which force the robot to move in the opposite direction not taking advantage of all the feasible directions, e.g. [8]. In particular, in [8] external distance sensors are distributed on a mobile robotic arm to sense its surroundings and reactively avoid collisions by modifying preplanned trajectories in the task space. Priority constraint based optimization is used in [9] for a robotic arm equipped with artificial skin consisting of multiple near-sighted proximity sensors with equality constraints for collision avoidance. In [10] inequality constraints are utilized; obstacles are sensed by distance sensors mounted on a robotic arm but no priority among the tasks and constraints is defined. Notice that the above works focus on single-arm robots.
In bimanual robots both collision avoidance with external obstacles and self-collision should be addressed. In [11] the danger field method is used for collision avoidance of a bimanual robot with external obstacles utilizing distance sensors. A task hierarchy is adopted where non-critical tasks are activated and deactivated depending on a scalar danger field metric, while evasive motions are projected into the null-space of the active tasks. Other works for bimanual robots with collision avoidance either assume known obstacle position or detect the obstacles visually. Self-collision avoidance and collision avoidance with known obstacles is addressed in [12] for a humanoid robot using a task priority strategy where each successive task is projected into the null space of the previous higher priority tasks. In our previous work [13], self and obstacle collision avoidance for a dualarm robot is addressed together with relative motion tasks and performance metrics optimization utilizing a priority constrained optimization method.
Mobile platforms are predominantly used to navigate the environment and move the robot to a specific location followed by the execution of the manipulation task without further utilization of the mobile platform. However, with this strategy the mobile platform is not fully exploited since apart from mobility it provides additional degrees of freedom (dofs) that may be used to satisfy secondary objectives and constraints. Mobile manipulation involves the consideration of the dofs provided by the mobile platform and the arm as one kinematic chain. Mobile manipulation with single arm robots has been proposed for a service robot in [14] and for door opening in [15].
Mobile bimanual pick-and-place tasks are addressed in [16]. However, it relies on planning rather than real-time motion generation given the current visual input which is used to reconstruct the environment. It is claimed that the real-time nature of this representation allows the robot to react by replanning to unforeseen obstacles.
In this work we propose a mobile dual-arm robot equipped with distance sensors distributed on its body and a kinematic control method for a mobile bimanual pick-and-place task in a dynamic environment. We restrict our analysis in cases the manipulation target is near the robot, e.g. within the field of view of the robot, as a navigation solution is out of scope of this work. The pick-and-place task involves a sequence of tasks with local, global and relative targets. We formulate each task as a priority constrained optimization problem that is solved online to avoid collisions with dynamic and static obstacles and achieve the goal. The primary contribution of this work is an integrated framework for mobile bimanual robots with distributed proximity sensors applied in bimanual pick-and-place tasks in the presence of static and dynamic obstacles. Proximity sensing for obstacle avoidance enables high responsiveness for reactive collision avoidance, but existing works have not so far fully exploited their potential. This work focuses on pick-and-place tasks, however this framework can be generalized to other tasks that can be defined using the subtasks and constraints described in the following sections.

II. PROBLEM DESCRIPTION AND CONCEPT SOLUTION
A mobile bimanual manipulator is inherently a system of multiple kinematic chains and subchains. Thus, mobile bimanual tasks may involve a number of motion tasks that are more conveniently defined with respect to different frames of reference. These are the world frame {w}, the mobile platform frame {m} and the left {l} and right {r} end-effector frames. By combining such motion tasks a robot is able to execute various complex tasks in many applications. Four types of motion tasks are distinguished on the basis of the reference frame used for their most convenient description: 1) The global end-effector motion task defined in {w} which can involve the left, right or both end-effectors (6 or 12 dofs), 2) the local end-effector motion task defined with respect to {m} which can involve the left, right or both endeffectors (6 or 12 dofs). Notice that in this case the target is invariant to the motion of the mobile platform, 3) the relative end-effector motion task (6 dofs) that is defined either in {l} or {r}, 4) the mobile platform motion task (3 dofs) that is defined in {w}. Additional tasks can be also defined on the basis of an optimization objective, such as velocity or force manipulability metrics for more efficient task execution. Examples of a global end-effector motion task is the unimanual or bimanual reaching of a pregrasp pose of an object located in the world. A local end-effector motion task is for example the placing of a grasped object on a tray that is located on the mobile platform. A relative end-effector motion task is the assembly of two small parts or the pose maintenance of a bimanually grasped object during transfer. Finally, a mobile motion task is the motion of the mobile platform to reach near a place of interest, e.g. a desk or workbench.
Moreover, effective mobile bimanual task execution in a dynamic environment involve a number of constraints that should be respected. In this work three types of constraints are detailed, namely: 1) Joint limit avoidance, that involves position and velocity constraints, 2) self-collision avoidance, that involves collision avoidance of the arms with each other and the mobile platform 3) obstacle collision avoidance, that involves collision avoidance with static and dynamic obstacles, like humans moving in the workspace of the robot. One can also list other types of constraints such as field of view, positioning of the center of mass, etc.
In this work a kinematic control approach with a hierarchical constrained optimization framework is adopted [17]. Robot tasks are defined by equality constraints which relate the desired velocity of the end-effector(s) to reach a global, local or relative target with the robot configuration space velocities via the forward kinematics of the appropriate kinematic chain. Inequality constraints are used to model structural or environmental constraints perceived by the sensors of the system, which should be respected during execution. The hierarchical optimization framework given equality and inequality constraints associated with an appropriate priority level, generates a configuration level velocity reference which is then commanded to the controllers of the robotic system. Thus, this control approach may be implemented on robots that offer position and/or velocity interfaces. Given all the equality and inequality constraints the robotic system cannot guarantee their satisfaction at all times, as conflicts between them may arise. The reference motion generated by a hierarchical optimization algorithm is optimal for the highest priority tasks, while lower priority tasks are satisfied only when they do not cause the violation of higher priority ones. Hence the definition of the appropriate priority between them influences the quality of the derived solution regarding the task objective.
In the hierarchical optimization framework the following hierarchical constrained optimization problem is solved at each control cycle to generate reference velocities for the robot dofs that optimally satisfy them.
where k denotes the index of a given priority level with 1 being the highest, n is the total number of hierarchical levels, matrix A k and vector b k describe the linear equality and inequality constraints, w k are slack variables used to relax the constraints in case of non-feasibility, while lexmin denotes that the solution is minimal with respect to a lexicographic order of priority [17]. Lastly,q is the velocity vector provided to the mobile dual-arm robot dofs. The position vector q may be decomposed to q = [q T l q T r q T m ] T , where q l , q r are the joint position vectors of the left and right arm respectively, while q m is the mobile platform dofs vector. Assuming an omnidirectional mobile platform moving on a planar surface its motion may be modelled as two prismatic and one revolute joints, thus q m ∈ R 3 . In this work we define the general framework for addressing the specific challenges related to mobile bimanual robot tasks in terms of the detailed design of tasks, constraints and hierarchies and test it in a complex bimanual pick-and-place operation. The implemented hierarchy is quite rich involving all the tasks and constraints defined in this section given the distributed sensing system of the robot. We thus demonstrate the effectiveness of the approach in a realistic non-trivial case. The following section details the formulation of equality and inequality constraints.

A. Equality constraints
In this work a kinematically controlled robot is assumed. Let a reference trajectory in SE(3) be described by the position p d and orientation Q d expressed as a unit quaternion, as well as their velocities v d and ω d respectively. A quaternion is given by Q = [η T ] T with η being the scalar and the vector part of the quaternion. The relation between the quaternion rateQ d and the rotational velocity ω d is given by: whereˆ is the skew symmetric matrix of the quaternion vector part and I 3 is the identity matrix of size 3. 1) Global end-effector motion task: In the global motion task, the reference trajectories involve motion generation to a target pose for the end-effector(s) with respect to {w}. To reach the target pose, end-effector reference trajectories can be either planned or generated in real-time by a dynamical system, e.g. a Dynamic Movement Primitive (DMP) [18].
In the hierarchical framework we construct equality constraints for this task by providing the kinematic relationship between the task velocities v di , ω di with i = {l, r} for the left and right end-effector andq: where J l , J r ∈ R 6×7 denote the respective individual arm Jacobians and J mi ∈ R 6×3 with i = {l, r} denotes the Jacobian which maps the mobile platform velocity to the end-effector velocity and 0 l , 0 r are zero matrices of the same dimensions as J l and J r respectively. Jacobian J mi has zero values in all coordinates of SE(3) that are not affected by the platform motion. In (2) the motion of an end-effector is generated by both the motion of the platform and the motion of the respective arm.
2) Local end-effector motion task : In case the task is defined in {m}, when for example the target is static with respect to {m} (for e.g. an object on the platform), it constitutes a local motion task and the respective Jacobian is: where the upper left superscript denotes the frame of reference {m} and 0 m is the zero matrix of the same dimensions as J mi . If a task involves only one end-effector, only one of the equality constraints in (2) or (3) is needed in this level. When both end-effectors are involved in this task type e.g. the reaching of a bimanual pre-grasping pose (12 dofs), we concatenate both the equality constraints of (2) or (3). Notice that the additional dofs of the platform generate a redundancy (total dofs ≥ 15) that facilitates the existence of a solution.
3) Relative end-effector motion task: In relative tasks the reference trajectory is expressed with respect to either the left {l} or the right {r} end-effector frame following [13]. In this work {r} is selected. The equality constraint on the relative motion task level is formed by utilizing the relative Jacobian which connects the relative velocities r v dl , r ω dl withq: where m J l , m J r denote the respective individual arm Jaco- where v dm are ω dm are the linear and rotational velocities of the mobile platform with respect to {w}. Note that the rows of J m , v dm and ω dm have non-zero values only for the two tangent and one normal directions to the planar surface of motion for the linear and rotational velocity respectively.

B. Self-collision Avoidance Constraints
For the purpose of self-collision avoidance a similar methodology as in our previous work [13] is followed to construct the constraints. The links and the body of the robot are modeled as capsules (Fig.1a). The method of Velocity Damper [19] is used to derive the general form of the constraint: where ε c is a positive damping parameter, d s is a safety distance between the two moving capsules, d ij = ||p i − p j || and n ij = (p i −p j )/||p i −p j || with p 1 and p 2 being the closest points between the two capsules ( Fig.1b) whose computation is analytically described in [20]. Matrices m J li , m J rj are the translational Jacobians of left and right robot arm considering the beginning of the capsule as end-effector transformed to the point p i and p j respectively; these Jacobians are augmented with zero columns for the subsequent joints. In case a collision pair involves a body capsule, which is static with respect to {m}, the respective Jacobian is zeroed out. Each capsule pair constitutes a single inequality constraint. Collision pairs that cannot collide due to their geometry and structural limits of the robot are removed to reduce the number of constraints in the solver. The utilization of distance sensors allows sensing of static and dynamic obstacles in the environment of the robot which in turn allows for environment-agnostic safe motion. Let f i denote a given distance sensor attached to the robot with i = 1...F where F is the total number of distance sensors. Let the position and unit view direction of sensor f i be p i (q) and n i (q) respectively, as seen in Fig.2. In this work, we use inequality constraints for sensor-based collision avoidance which are formulated as follows:

C. Obstacle Collision Avoidance constraints
with d i being the distance measured, d si a preset safety distance associated with f i , ε r is a positive damping parameter and J li , J ri , J mi are the translational Jacobians mapping the left, right arm joint velocities and the mobile platform velocity respectively to the velocity of a point where sensor f i is located. Notice that the measured distance d i is the distance from the sensor to the closest intersection point between the measurement cone of the sensor and the obstacle (Fig.2). It is important to identify whether the sensors view an external obstacle or a part of the robot. If the measured distance corresponds to a point obstacle that lies inside a collision capsule of the robot, it is identified as a selfmeasurement and the corresponding constraint is deactivated.

D. Joint Limit Avoidance
Finally, joint limits are strict limits of the robotic hardware and must be respected at all costs. Joint limits usually include position and velocity limits. Their constraints can be implemented by damping the joint velocities near their mechanical limits as well as near their velocity limits via the following inequalities: where ε l are positive damping parameters, q min , q max are the lower and upper joint limits respectively, V q is the vector of the absolute joint velocity limits limits, while max and min denote the element-wise maximum and minimum of the provided vectors respectively.

IV. MOBILE BIMANUAL PICK-AND-PLACE APPLICATION
The above framework has been applied in a mobile bimanual pick-and-place task utilizing the various tasks described in Sec. II. In particular, a mobile motion task was utilized in the initiation of the pick-and-place task to reach near the pick-up location and also to reach the place-down location. Global motion tasks were utilized to reach the pregrasping poses and in combination with a relative motion task to grasp, to reach a pre-release pose and to release the object. A local in combination with a relative motion task was used to lift and retract the grasped object. Furthermore, to ensure the safety of the robot and the environment the constraints described in Sec. II were also utilized. Joint limit constraints, self-collision constraints and obstacle collision avoidance constraints along with the motion tasks were incorporated in a prioritized scheme in order to complete the pick-and-place task. Violation of joint limits not only generates infeasible motion, but can also damage the robot, while violation of the collision avoidance constraints might damage the robot and the environment. Thus, it is reasonable to adopt the following hierarchical order of constraints in descending order: where c jl , c col refer to joint limits and collision avoidance that includes self and dynamic obstacle collision avoidance, while c rel , c glo , c loc and c mob refer to equality constraints for relative, global, local and mobile tasks respectively. Task c rel has the highest priority among the equality constraints since its violation may cause drop or squeeze of the grasped object while the lowest priority equality constraint is c mob since the mobile platform pose is a secondary objective in comparison to the pose of the end-effectors in this task.

A. Experimental setup and implementation details
Our hardware setup consists of a YuMi bimanual robot mounted on a Ridgeback mobile platform (Fig.3a). Furthermore, 16 VL6180X distance sensors are placed on the arms of the robot (Fig.3b) taking measurements every 40ms, as well as two Hokuyo UST-10LX lidar sensors are installed on the Ridgeback to perceive obstacles in the environment. Each of the utilized lidar sensors is offering 270 o measurement angle with 0.25 o resolution every 25ms. Each such measurement constitutes a single obstacle collision avoidance constraint resulting in 2178 external collision avoidance constraints for all the utilized distance sensors. However only a small subset of them is active at every control cycle depending on the environment allowing for real-time performance.
The distribution of the sensors on the robot shown in Fig.3b was selected empirically, however a more optimized distribution may be computed through an optimization algorithm [21]. The calibration of the pose of the sensors with respect to the mobile frame {m} was done manually through a graphical user interface. Furthermore, the AMCL ROS package was utilized for the localization of the mobile platform in a known map using lidar measurements and the odometry of the wheels. Finally, the constrained optimization problem formulated in the previous sections is solved using the HQP solver [17] as it is capable of computing a solution in real-time in similarly sized problems.
The reference trajectories for c glo , c loc , c rel and c mob are generated by utilizing a second order dynamical system both for translation and rotation given by: where α = 5, β = 0.8 are positive constant gains and p T , Q T the target position and orientation expressed as a unit quaternion. The system is integrated to provide a reference translational and rotational velocity v d , ω d ∈ R 3 and the reference position and orientation expressed as a unit quaternion p d , Q d . Variables η e and e in (11) denote the scalar and vector part of the quaternion error Q d * Q −1 T . During execution static and dynamic obstacles, tables and human respectively, are present in the scene. Safety distance values d s are chosen empirically to trade off the distance required to allow reaching the pick-and-place locations with avoidance of dynamic obstacles. For the sensors on the wrist the safety distance is 0.1m, on the forearm 0.15m and on the mobile platform it ranges from 0.25m to 0.45m depending on their location. Damping parameters in (6), (7), (8) and (9) are set at values ε r = 2, ε c = ε l = 0.5, to allow a smooth reaction. The control cycle period is set to 8ms as a solution to the optimization could be reached under this time period. In cluttered scenes where the number of the active constraints increases, the solver may take more than 8ms to reach an optimal solution. In such cases the solver was interrupted prematurely at 8ms and the current suboptimal solution was considered without noticeable degradation of the quality of the solution.

B. Pick-and-place experiment
The goal of the experiment is to grasp bimanually an object from the pick-up location and release it on top of the container (place-down location) shown in the snapshots of Fig.4. The pick-and-place task can be segmented in multiple motion sequences that must be executed sequentially to successfully complete it. The target key-frames of each task may be provided by a higher level planning module or extracted by learning from demonstration [22]. In this experiment the object pose, pick-up and place-down location are considered known. The experiment is initiated with a mobile motion task in order to position the robot close to the pick-up location. At t = 10s during the mobile motion a person walks near the robot and disturbs its task (Fig.4a). The robot senses the human with the distance sensors and avoids collision with him by modifying the mobile platform motion and by moving the robot arms away from collision. Two global motion tasks follow, one per end-effector, in order to reach a pregrasping pose. Then while the right end-effector global task is active, a relative motion task is executed to grasp the object. This relative motion task is maintained until the object release. Once the object is grasped, a local motion task is executed in order to lift and retract the grasped object, followed by a mobile motion task to reach close to the place-down location. At t = 65s once again a person disturbs the robot which avoids him (Fig.4b). A global motion task is executed to reach a prerelease and a release pose and finally a relative motion task to release the object (Fig.4c) .
The time evolution of the right end-effector with respect to {w} and the relative pose between the two end-effectors are given by Fig.5 and Fig.6 respectively. The green and cyan vertical dotted lines denote the initiation of a task and the human disturbance respectively, while the dashed lines denote the commanded target pose. It is clear that the target keyframes are successfully reached. In Fig.7   away to avoid possible collisions. Notice that at t = 65s in Fig.5 and Fig.6 the relative pose is maintained while the right arm pose is disturbed by the presence of the human. This happens due to the higher priority of the relative task compared to the right arm task. In comparison at t = 10s where there is no active relative or end-effector task, the relative pose Fig.6 is visibly disturbed to avoid collision. Slight violations occur momentarily due to low measurement and control frequency with respect to human speed. After 85s the table is sensed (left:d 1 and right: d 2 , d 3 ). The rest of the measurements correspond to self-measurements.

C. Pose maintenance experiment
In order to further demonstrate the efficacy of the proposed method in case of demanding tasks such as maintaining the pre-release pose, an additional experiment was performed. In particular, the robot is commanded to maintain the right end-effector and the relative pose at at their initial values.
The human is purposely approaching the mobile platform multiple times (at 7, 10, 12, 20, 22 and 25s) as seen in Fig.8. The robot is avoiding the person while the arms are adjusting their configuration in order to keep the relative pose and right end-effector pose unaffected. The relative pose is maintained with position errors less than 10 −3 m and orientation errors less than 5·10 −3 rad. The right end-effector orientation is maintained with error less than 5 · 10 −3 rad, but the position error may reach up to 2.5 · 10 −2 m indicating considerable constraint violations. This can be explained by the lower priority of the right end-effector task with respect to the relative task. Nevertheless, it converges to the desired target as soon as the higher priority constraints allow it. Also, small violations of the distance sensor constraints occur only momentarily for the same reasons discussed in the previous experiment.

V. CONCLUSIONS
In this work we formulate the pick-and-place task as a sequence of mobile manipulation tasks with relative, global and local targets in each task. Constraints for avoidance of joint limits, self-collisions and dynamic obstacles as well as equality constraints for task goals are defined in a hierarchical order. A hierarchical constrained optimization problem is then solved at each control cycle to generate reference velocities for the robot dofs that optimally satisfy them. The robot perception of the environment is robot centric with multiple distance sensors placed on the robot. Experiments with the YuMi robot mounted on the Ridgeback mobile platform demonstrate the efficacy of the proposed method even in case of demanding tasks. The developed methodology is modular thus allowing our framework to be easily adapted to new mobile bimanual robotic tasks.