We present a methodology for estimating joints torque due to external forces applied to a robot with large joints backlash and friction. This undesired non-linearity is common in personal robot, due to the use of low cost mechanical components and type of usage. Our method enables contact detection and human-robot physical interaction capabilities without using extra sensors. The effectiveness of our approach is shown with experiments on a Romeo robot arm from SoftBank Robotics.

We consider the problem of extracting a complete set of numerical parameters that characterize the robot dynamics, starting from the identified values of dynamic coefficients that linearly parametrize the robot dynamic equations. This information is relevant when realistic dynamic simulations have to be performed using standard packages, or when addressing the efficient numerical implementation of model-based control laws using recursive Newton-Euler algorithms. The formulated problem is highly nonlinear and is solved through the use of global optimization techniques, while imposing also physical bounds on the dynamic parameters. The identification and parameter extraction process is illustrated and experimentally validated on the link dynamics of a KUKA LWR IV+ robot.

During human-robot interaction tasks, a human may physically touch a robot and engage in a collaboration phase with exchange of contact forces and/or requiring coordinated motion of a common contact point. Under the premise of keeping the interaction safe, the robot controller should impose a desired motion/force behavior at the contact or explicitly regulate the contact forces. Since intentional contacts may occur anywhere along the robot structure, the ability of controlling generalized contact motion and force becomes an essential robot feature. In our recent work, we have shown how to estimate contact forces without an explicit force sensing device, relying on residual signals to detect contact and on the use of an external (depth) sensor to localize the contact point. Based on this result, we introduce two control schemes that generalize the impedance and direct force control paradigms to a generic contact location on the robot, making use of the estimated contact forces. The issue of human-robot task compatibility is pointed out in case of control of generalized contact forces. Experimental results are presented for a KUKA LWR robot using a Kinect sensor.

Our recently developed Reverse Priority (RP) redundancy resolution method is extended here to the presence of unilateral constraints. The RP method computes the solution to a stack of prioritized tasks starting from the lowest priority one, and adding iteratively the contributions of higher priority tasks. In this framework, unilateral constraints can be added efficiently, while guaranteeing also continuity of joint velocity commands. Since unilateral (hard) constraints are typically placed at the highest priority levels, their treatment within the RP method leads to the least possible modification of the solution computed so far, when analyzing the need to activate or not these constraints. The effectiveness of the approach is shown by simulations on a planar 6R robot and on a humanoid robot, as well as experiments on a KUKA LWR manipulator.

We present an efficient method for addressing online the inversion of differential task kinematics for redundant manipulators, in the presence of hard limits on joint space motion that can never be violated. The proposed Saturation in the Null Space (SNS) algorithm proceeds by successively discarding the use of joints that would exceed their motion bounds when using the minimum norm solution. When processing multiple tasks with priority, the SNS method realizes a preemptive strategy by preserving the correct order of priority in spite of the presence of saturations. In the single- and multitask case, the algorithm automatically integrates a least possible task-scaling procedure, when an original task is found to be unfeasible. The optimality properties of the SNS algorithm are analyzed by considering an associated quadratic programming problem. Its solution leads to a variant of the algorithm, which guarantees optimality even when the basic SNS algorithm does not. Numerically efficient versions of these algorithms are proposed. Their performance allows real-time control of robots executing many prioritized tasks with a large number of hard bounds. Experimental results are reported.

The capability of controlling both the position/torque and the stiffness of the joints is the main feature of the next generation of robots based on Variable Stiffness Actuators (VSA). For the purpose of accurate control, recent works have pointed out that is not possible to rely completely on analytical models of the stiffness characteristics of the transmissions/joints and that an on-line estimation of stiffness is often mandatory. Building on our previous results, we present a new method to estimate the stiffness based only on input-output signals, without any knowledge of motor parameters nor the need of joint torque sensing. In addition, a Recursive Least Squares method based on a QR decomposition (QR-RLS) is used, which is very robust to poor excitation conditions. In order to deal more efficiently with noisy signals, a combination of two filtering actions is also considered, with a causal Kinematic Kalman Filter (KKF) and a non-causal Savitzky-Golay (SG) filter. Simulation results and comparison with two other state-of-the-art stiffness estimators are presented.

The paper addresses the following problem for redundant robots. Given a second-order inverse differential scheme that realizes instantaneously a desired task acceleration and has some specified properties in terms of joint acceleration or torque, define a discrete-time joint velocity command that shares the same characteristics under suitable hypotheses. The goal is to obtain simpler implementations of possibly complex robot control laws that i) can be directly interfaced to the low-level servo loops of a robot, ii) require less task information and on-line computations, iii) are still provably good with respect to some target performance. The method is illustrated by considering the conversion into discrete-time velocity commands of control schemes for redundant robots that minimize the (possibly, weighted) norm of joint acceleration or joint torque, or that add null-space damping to overcome floating motion of the robot joints. Numerical results are presented for the kinematic control of a 7R KUKA LWR.

An approach is presented for the model identification of the so-called link dynamics used by the KUKA LWR-IV, a lightweight manipulator with elastic joints that is very popular in robotics research but for which a complete and reliable dynamic model is not yet publicly available. The control software interface of this robot provides numerical values of the link inertia matrix and the gravity vector at each configuration, together with link position and joint torque sensor data. Taking advantage of this information, a general procedure is set up for determining the structure and identifying the value of the relevant dynamic coefficients used by the manufacturer in the evaluation of these robot model terms. We call this a reverse engineering approach, because our main goal is to match the numerical data provided by the software interface, using a suitable symbolic model of the robot dynamics and the inertial and gravity coefficients that are being estimated. Only configuration-dependent terms are involved in this process, and thus static experiments are sufficient for this task. The main issues of dynamic model identification for robots with elastic joints are discussed in general, highlighting the pros and cons of the approach taken for this class of KUKA lightweight manipulators. The main identification results, including training and validation tests, are reported together with additional dynamic validation experiments that use the complete identified model and joint torque sensor data.

A novel method to handle multiple robotic tasks with priorities is presented. The occurrence of singularities, both of the kinematic and algorithmic type, may affect the correct hierarchy in task execution. Existing methods deal with singularities either by using damped least squares solutions or by relaxing the enforcement of secondary tasks. Damped pseudo-inversion mitigates undesired effects near singularities, at the cost of non-negligible task errors and deformation even of the highest priority task. When secondary tasks are not enforced, hierarchy is preserved but these tasks are not executed accurately even when this would be possible. In our approach, joint motion contributions are added following the reverse order of task priorities and working with suitable projection operators. Higher priority tasks are processed at the end, avoiding possible deformations caused by singularities occurring in lower priority tasks. The proposed Reverse Priority (RP) method allows executing at best all tasks while still preserving the desired hierarchy. The effectiveness of the RP method is shown through numerical simulations and with experiments on a 7-dof KUKA LWR.

Physical human-robot collaboration is characterized by a suitable exchange of contact forces between human and robot, which can occur in general at any point along the robot structure. If the contact location and the exchanged forces were known in real time, a safe and controlled collaboration could be established. We present a novel approach that allows localizing the contact between a robot and human parts with a depth camera, while determining in parallel the joint torques generated by the physical interaction using the so-called residual method. The combination of such exteroceptive sensing and model-based techniques is sufficient, under suitable conditions, for a reliable estimation of the actual exchanged force at the contact, realizing thus a virtual force sensor. Multiple contacts can be handled as well. We validate quantitatively the proposed estimation method with a number of static experiments on a KUKA LWR. An illustration of the use of estimated contact forces in the realization of collaborative behaviors is given, reporting preliminary experiments on a generalized admittance control scheme at the contact point.

For robots that are redundant with respect to a given task, we present an optimal differential kinematic inversion method in the presence of hard bounds on joint range, joint velocity, and joint acceleration. These hard bounds specify the robot motion capabilities that cannot be exceeded at any time. On the other hand, scaling of the desired task trajectory is allowed whenever the robot capabilities are insufficient to execute the original task. For a problem formulated in this way, we have recently presented the Saturation in the Null Space (SNS) algorithm that produces an efficient solution, based on Jacobian pseudoinversion and recovery in the null space of the saturation effects of a reduced number of joint velocity commands. To investigate the optimality properties of the SNS algorithm, we recast the problem as a constrained quadratic programming (QP) problem, in which the joint velocity norm as well as the task scaling are to be minimized. Its solution leads to a variant of the original algorithm, the Optimal Saturation in the Null Space (Opt-SNS). The Opt-SNS guarantees an optimal solution also when the basic SNS fails to do so and improves the numerical performance over the state-of-the-art QP solver. The possible existence of discontinuous solutions for the formulated problem is avoided by the introduction of a task scaling margin. The extension to the multi-task case is also presented. Simulation results for the 7R lightweight KUKA LWR IV robot illustrate the properties and computational efficiency of the new algorithm.

In physical Human-Robot Interaction, the basic problem of fast detection and safe robot reaction to unexpected collisions has been addressed successfully on advanced research robots that are torque controlled, possibly equipped with joint torque sensors, and for which an accurate dynamic model is available. In this paper, an end-user approach to collision detection and reaction is presented for an industrial manipulator having a closed control architecture and no additional sensors. The proposed detection and reaction schemes have minimal requirements: only the outer joint velocity reference to the robot manufacturer's controller is used, together with the available measurements of motor currents and joint positions. No a priori information on the robot dynamic model and existing low-level joint controllers is strictly needed. A suitable on-line processing of the motor currents allows to distinguish between accidental collisions and intended human-robot contacts, so as to switch the robot to a collaboration mode when needed. Two examples of reaction schemes for collaboration are presented, with the user pushing/pulling the robot at any point of its structure (e.g., for manual guidance) or with a compliant-like robot behavior in response to forces applied by the human. The actual performance of the methods is illustrated through experiments on a KUKA KR5 manipulator.

A kinematically redundant robot with limited motion capabilities, expressed by inequality constraints of the box type on joint variables and commands, needs to perform a set of tasks, expressed by linear equality constraints on robot commands, possibly organized with priorities. Robot motion capabilities cannot be exceeded at any time, and the resulting constraints are to be considered as hard bounds. Instead, robot tasks can be relaxed by velocity scaling if no feasible solution exists. To address this redundancy resolution problem, we developed a method in which joint space commands are successively saturated and their effect compensated in the null space of a suitable task Jacobian (SNS, Saturation in the Null Space). Computationally efficient versions of the basic and optimal SNS algorithms are proposed here, based on a task augmentation reformulation, a QR factorization of the main matrices involved, and a so-called warm start procedure. The obtained performance allows to control in real time robots with high-dimensional configuration spaces executing a large number of prioritized tasks, and with an associated high number of hard bounds that saturate during motion.

The video illustrates on-going activities at DIAG Sapienza on physical Human-Robot Collaboration (pHRC), based on a control framework imposing robot behaviors that are consistent with safety and coexistence requirements.

We present a new image-based visual servoing scheme for tracking moving targets. This is achieved with a twofold approach. First, we devise a straightforward adaptation of a previously proposed depth observer to account for the fact that the target is not stationary. Second, we estimate the disturbance on the visual feature dynamics due to the target motion, and we add a related compensation term to the visual controller. In particular, the target velocity components parallel to the image plane are reconstructed using a disturbance observer, whereas the orthogonal component is retrieved from the measurement of the Focus Of Expansion. Comparative experiments show that the proposed method can improve over classical visual servoing schemes by 50% or more.

In this paper a real-time collision avoidance approach is presented for safe human-robot coexistence. The main contribution is a fast method to evaluate distances between the robot and possibly moving obstacles (including humans), based on the concept of depth space. The distances are used to generate repulsive vectors that are used to control the robot while executing a generic motion task. The repulsive vectors can also take advantage of an estimation of the obstacle velocity. In order to preserve the execution of a Cartesian task with a redundant manipulator, a simple collision avoidance algorithm has been implemented where different reaction behaviors are set up for the end-effector and for other control points along the robot structure. The complete collision avoidance framework, from perception of the environment to joint-level robot control, is presented for a 7-dof KUKA Light-Weight-Robot IV using the Microsoft Kinect sensor. Experimental results are reported for dynamic environments with obstacles and a human.

We present a novel efficient method addressing the inverse differential kinematics problem for redundant manipulators in the presence of different hard bounds (joint range, velocity, and acceleration limits) on the joint space motion. The proposed SNS (Saturation in the Null Space) iterative algorithm proceeds by successively discarding the use of joints that would exceed their motion bounds when using the minimum norm solution and reintroducing them at a saturated level by means of a projection in a suitable null space. The method is first defined at the velocity level and then moved to the acceleration level, so as to avoid joint velocity discontinuities due to the switching of saturated joints. Moreover, the algorithm includes an optimal task scaling in case the desired task trajectory is unfeasible under the given joint bounds. We also propose the integration of obstacle avoidance in the Cartesian space by properly modifying on line the joint bounds. Simulation and experimental results reported for the 7-dof lightweight KUKA LWR IV robot illustrate the properties and computational efficiency of the method.

We present an efficient method for motion control of redundant robots performing multiple prioritized tasks in the presence of hard bounds on joint range, velocity, and acceleration/ torque. This is an extension of our recently proposed SNS (Saturation in the Null Space) algorithm developed for single tasks. The method is defined at the level of acceleration commands and proceeds by successively discarding one at a time the commands that would exceed their bounds for a task of given priority, and reintroducing them at their saturated levels by projection in the null space of a suitable Jacobian associated to the already considered tasks. When processing all tasks in their priority order, a correct preemptive strategy is realized in this way, i.e., a task of higher priority uses in the best way the feasible robot capabilities it needs, while lower priority tasks are accommodated with the residual capability and do not interfere with the execution of higher priority tasks. The algorithm automatically integrates a multi-task least possible scaling strategy, when some ordered set of original tasks is found to be unfeasible. Simulation and experimental results on a 7-dof lightweight KUKA LWR IV robot illustrate the good performance of the method.

We propose a novel approach for estimating the nonlinear stiffness of robot joints with flexible transmissions. Based on the definition of dynamic residual signals, we derive stiffness estimation methods that use only position and velocity measurements on the motor side and needs only the knowledge of the dynamic parameters of the motors. In particular, no extra force/torque sensing is needed. Two different strategies are considered, a model-based stiffness estimator and a black-box stiffness estimator. Both strategies consist of two stages. The first stage of the model-based estimator generates a residual signal that is a first-order filtered version of the flexibility torque of the transmission, while in the second stage a least squares fitting method is used to estimate the model parameters of the stiffness. The black-box estimator uses in the first stage a second-order residual that is directly a filtered version of the stiffness multi plied by the deformation rate of the transmission. In the second stage, a simple regressor provides the transmission stiffness in a singularity-robust way. Numerical results reported for the cases of constant, nonlinear, or variable stiffness transmissions demonstrate the effectiveness of the approach and the relative merits of the two estimation strategies.

We present a new control approach to regulation tasks for robots with elastic joints in the presence of gravity. The control law combines a term that cancels the gravity effects on the robot link dynamics with a PD-type error feedback on the motor variables. The first control component follows from the feedback equivalence principle when imposing to the link variables the same dynamic behavior as if gravity were absent. The PD component can then be designed in a rather straightforward way. Global asymptotic stability is shown via Lyapunov analysis, without the need of strictly positive lower bounds neither on the proportional control gain nor on the structural joint stiffness. The control approach is also extended to the case of robot joints with nonlinear stiffness.

Affine-invariant feature matching plays an important role in many robot vision applications, such as robot visual navigation, object detection, visual tracking and visual SLAM, etc. In the early stages, invariant keypoints are used to detect the affine transformation. But the accuracy is very low. In recent years, some people introduce SIFT method into robot vision field, which greatly enhances the accuracy. But it is too time-consuming to meet the requirements of real-time robot vision applications. In this paper, we propose a novel learning-based feature matching approach to address the problem. First, it uses a fast algorithm to extract keypoints. Then, our method identifies keypoints that belong to different objects or background by color and texture representation. The keypoints are clustered into corresponding groups. At last, a two-stage multilayer ferns classifier is trained to recognize the local patches and get the estimate of viewpoint. We test our approach on public datasets and apply it in a visual SLAM application. The result demonstrates that our method can provide robust and powerful matching ability. Even on some difficult matching cases, it also performs remarkably well. Further more, because there is no need to compute descriptors for the image, our method is very fast at run-time.

Depth and presence sensors are used to prevent collisions in environments where human/robot coexistence is relevant. To address the problem of occluded areas, we extend in this paper a recently introduced efficient approach for preventing collisions using a single depth sensor to multiple depth and/or presence sensors. Their integration is systematically handled by resorting to the concept of image planes, where computations can be suitable carried out on 2D data without reconstructing obstacles in 3D. To maximize the on-line collision detection performance by multiple sensor integration, an off-line optimal sensor placement problem is formulated in a probabilistic framework, using a cell decomposition and characterizing the probability of cells being in the shadow of obstacles or unobserved. This approach allows to fit the optimal numerical solution to the most probable operating conditions of a human and a robot sharing the same working area. Three examples of optimal sensor placement are presented.

In this paper we discuss the integration of active and passive approaches to robotic safety in an overall scheme for real-time manipulator control. The active control approach is based on the use of a supervisory visual system, which detects the presence and position of humans in the vicinity of the robot arm, and generates motion references. The passive control approach uses variable joint impedance which combines with velocity control to guarantee safety in worst-case conditions, i.e. unforeseen impacts. The implementation of these techniques in a 3-dof, variable impedance arm is described, and the effectiveness of their functional integration is demonstrated through experiments.

Variable stiffness actuation (VSA) devices are being used to jointly address the issues of safety and performance in physical human-robot interaction. With reference to the VSA-II prototype, we present a feedback linearization approach that allows the simultaneous decoupling and accurate tracking of motion and stiffness reference profiles. The operative condition that avoids control singularities is characterized. Moreover, a momentum-based collision detection scheme is introduced, which does not require joint torque sensing nor information on the time-varying stiffness of the device. Based on the residual signal, a collision reaction strategy is presented that takes advantage of the proposed nonlinear control to rapidly let the arm bounce away after detecting the impact, while limiting contact forces through a sudden reduction of the stiffness. Simulations results are reported to illustrate the performance and robustness of the overall approach. Extensions to the multidof case of robot manipulators equipped with VSA-II devices are also considered.