Autonomous Industrial Assembly using Force, Torque, and RGB-D sensing

In this paper, we perform a detailed experimental study on three selected tasks from the assembly challenge that are representative of “peg-in-hole” and “hole-on-peg” assemblies, for which we present two strategies, spiral-based search and tilt-based insertion, each of which applies to both cases. We demonstrate how these primitives can be combined with 3D perception to identify part, hole and pegs, and other force-based primitives to perform complex, multi-step assemblies. We record both torque and force measurements over time for both successful and failed assemblies, which are available online¹¹1https://github.com/RoboticMaterials/wrs2018-paper, and discuss future directions of research that will help increase performance of autonomous assembly of industrial products.

The setting for the tasks is a base plate 1 cm thick acetal resin that has holes for mounting the free components used on the 2D assembly task. The plate is supported by four standoffs which are each 3 cm high. There are three tasks in the experiment: Large Bearing Task, Small Pulley Task, and Stud Task, which are taken from the Taskboard Challenge at the World Robotics Summit.

The goal of the large bearing task is to lower a flanged bearing with housing (Misumi part SBARB6200ZZ-30) into a hole in the base plate. The hole in the base ($\oslash 36+0.2mm$) is a sliding fit with the large bearing, such that an insertion attempt that is out of alignment will easily jam the bearing and prevent insertion. This task is representative for a “peg-in-hole” task.

For the small pulley task, a pulley wheel 3.2 cm in diameter (Misumi part MBRFA30-2-P6) is placed onto a 6 mm diameter shaft that is 5.2 cm long (Misumi part PSSFAN6-50-F10-B8-P4). This task is representative for a “hole-on-peg” task.

In order to accomplish the large nut task, the nut (Misumi part SLBNR12) must be threaded onto a stud (Misumi parts SCB12-25, screw, and SPWF12, washer) in the base plate. This task is a combination of a “hole-on-peg” task combined with a screwing task.

Control of the system is provided by a Robotic Materials SmartHand. The SmartHand is an integrated computing, vision, and parallel gripper platform designed for use with serial, collaborative [12] robots. It integrates an nVidia Jetson TX2 computer for control and image processing. Visual sensory data comes from an Intel RealSense D430 that is mounted in the palm. The RealSense is a structured-light camera that projects two infrared patterns into the field of view of the image sensor, and calculates per-pixel depth using interferometry, resulting into a Red-Green-Blue plus Depth image (RGB-D). Both RGB-D and infrared images can be obtained from the sensor. The camera and computer are mounted to an internal frame that provides passive cooling. Smarthand interacts with its environment through a parallel gripper. The fingers of the gripper have a tapered profile that mimics the beak of a crow, a design choice based on the renown of corvids for dextrous manipulation [13]. Also like a crow, the vision sensor’s field of view is in the immediate workspace of the gripper, allowing the gripper to see assembly objects as close as 11cm.

The SmartHand is mounted to an OptoForce (now OnRobot) HEX-E 6-axis force-torque (FT) sensor capable of sensing the wrench at the wrist joint with a resolution of 0.2 Newtons in X-Y and 0.5 Newtons in Z direction for force measurements and 0.010 in X-Y and 0.002 Newton-meters around the Z direction for torque measurements.

The FT sensor is mounted to a Universal Robotics UR5 serial manipulator. The UR5 employs sensitive motor torque feedback to a safety system that prevents it from imparting harmful forces to humans and objects in its workcell. Thus, UR5 is considered to belong to a class of machines known as collaborative robots [14, 15], which have been steadily gaining popularity in both industry [16] and research [17].

The control software used for this work is an extension of the RMStudio software, which is a collection of Python 3 libraries for interface with the UR5 robot and SmartHand via Jupyter Notebooks. Here, all sensors and actuators are abstracted into Python libraries that communicate with the sensor and actuator programming interfaces using XML-RPC, RTDE and socket communications, resulting into a single Python script that controls all operations. The Jupyter notebook allows the programmer to execute code step-by-step, visualizing output inline with code, which supports rapid prototyping.

Parts and holes are located using appropriate methods from the Open3D and OpenCV libraries.

To move the robot, we make use of UR’s inverse kinematics interface, which allows us to move the robot in Cartesian space. Here, we avoid collisions by moving the robots to safe way-points above the task board, which we assume to remain unobstructed during task progress.

Modularization and robustness are our main goals in the design of assembly actions. By breaking up tasks into common actions, we are able to reuse actions for multiple tasks. Halt et al. [6] identify 6 primitive actions in the execution of assembly tasks. We have designed the actions for this system to be robust to small misalignment and displacement, so that single-purpose jigging can be avoided as much as possible. Presently, our system has two categories of actions that meet this need: perception steps and join actions.

Perception steps are designed to obtain a more accurate pose estimate of a part that will participate in a later join action. We have created two different perception steps that have wide applicability to the assembly tasks that comprise the competition assembly problems. The first is a circular feature locator, CircleLocator. This has obvious uses for fasteners, shafts, and the associated holes. Fasteners comprised a large portion of the WRS Challenges, so this function was deployed repeatedly in our solution to the assembly problem. The hole pose locator makes use of the Circle Hough Transform [18] and camera intrinsics in order to obtain a hole pose (Figure 2, left). The circle locator takes the diameter of the queried feature and the distance between the plane of the circle and the gripper camera as parameters. CircleLocator assumes that the camera begins the procedure with the line of sight of the camera pointed in a direction perpendicular to the circle feature plane. It uses the RealSense field of view (FOV) parameters to calculate how large the circle should appear in the captured image. Although this has wide use, it has the drawback of fixing only 2 of the 3 SO(3) degrees of freedom. For these cases, the point cloud feature locator provides a means for us to match a known point model with a perceived model. This method relies on iterative closest point (ICP) [19], to match a user-recorded example point cloud in a desired orientation to a point cloud scanned by the hand camera.

The above part location strategies proved useful for providing finer locations of parts in an environment where the parts are arranged by hand. Even so, there was still variation in how the parts were gripped by the robot. In order to account for this, we developed part placement actions designed to be robust against these variations. There are two actions in this category: spiral and tilt-in. Both of these actions are suitable for peg-in-hole and hole-on-peg operations, which are the focus of this work.

The Pickpart procedure is designed as a simple part grasping action. It takes the center of the grasp ($\vec{x}_{PartLocation}$), diameter of the part at the grasp pose (Dia_Bearing), and depth from the part surface ($z_{PickDepth}$) as parameters. $z_{PickDepth}$ is expressed in the gripper’s local frame, and is used to tune how much of the gripper finger surface engages the part. Once the gripper reaches the part location and applies the offset, the gripper closes as fully as possible. This procedure does not provide feedback as to whether the part was grasped.

Although it is not a placement action, exactly, we designed TampWithForceLimit as a compensatory procedure to deal with part jamming that might occur during assembly. It simply moves the gripper to an absolute location $\vec{x}_{loc}$, then moves the gripper down in its local $-Z$ axis until either the movement limit $\Delta z$ is reached, or the wrist sensor detects a reaction force that exceeds the user-specified limit $F_{touch}$. It then moves the gripper back along the approach vector in the opposite direction by $\Delta z$.

Twist is another compensatory procedure; it is meant to free a part that has mechanically bound (jammed) on its fulcrum. It is designed to rotate the bound part about its fulcrum in an oscillatory way. Twist takes the grasp center $\vec{x}_{loc}$, the angular oscillation magnitude $\theta_{osc}$, and the number of oscillations $N_{osc}$ as parameters. $\vec{x}_{loc}$ must be defined so that the local gripper $Z$-axis is aligned with the fulcrum axis in the lab frame. The procedure moves the gripper to the grasp center, closes the gripper, then sweeps the wrist $Z$-axis rotation by $\pm\theta_{osc}$ for $N_{osc}$ repetitions.

The Thread procedure is used for the nut threading task. It takes the beginning nut pose $\vec{x}_{loc}$, number of flats to advance each turn $N_{flats}$, tightening torque limit $T_{z,limit}$, and fastener thread pitch in $d_{pitch}$ as parameters. The procedure begins by closing the gripper at the grip center, assuming that $\vec{x}_{loc}$ has been defined with an orientation that places the gripper finger surfaces on flats of the nut. Then, in a loop, the gripper rotates by $-60N_{flats}$ degrees (clockwise) while moving in $-Z$ by the thread pitch, opens, and then rotates back by $60N_{flats}$ degrees counterclockwise. The loop exits when the $T_{z,limit}$ is reached. $T_{z,limit}$ is chosen so that then nut is finger tight. This limit should be encountered whether the nut has reached the base plate or cross-threaded.

The spiral part insertion algorithm is provided in pseudo-code in Algorithm 1 and illustrated in Figure 3. For the spiral action, the held part is brought towards the reference part along the axis of insertion at the perceived location of the hole until a contact force is sensed. For this, the robot first moves to an absolute position (MoveAbs()) given by the insertion pose $\vec{x}_{i}$ plus an offset $\Delta z$ above $\vec{x}_{i}$. Note, that $\vec{x}_{i}$ is a 6-DoF pose, which encodes not only a 3D point, but also the pose of the insertion plane. The robot then moves towards $\vec{x}_{i}$ along the insertion axis until it has either traveled $\Delta_{max}$ or the function MaxDownForce does not evaluate true given a certain threshold. This is indicated by the left arrow construct. Using a reasonable value $\Delta_{max}>\Delta z$, the robot moves with speed $v$ toward the insertion location, until a certain contact force $-F_{t}$ is exceeded. The robot then performs a spiraling motion MoveSpiral() with step-size $\Delta s$, maximum radius $r_{max}$, speed $v$ and acceleration $a$ as long as MaxSpiralForces() returns true. The spiral equation is given by

where ($x_{0},y_{0}$) is some starting position, $r_{i}$ is a radius incremented every step, and $\phi_{i}$ is an angle updated every step.

The intent of this operation is to use the end of the held part to search the surface of the stationary part for the actual location of the hole. When the held part reaches the hole, then the downward pressure causes the part to drop into the hole. When the held part has entered the hole, it is confined on two translation axes, leading to excessive lateral forces and torques. Specifically, this function is written to detect excessive torques (here: 0.9 Nm) in either direction around the X and Y axes, as well as pressure forces exceeding $F_{d}$. Should these events occur, it is likely that the part has snapped into its final location and even small movements immediately lead to large torques or contact forces.

The tilt-in action is also designed for cylindrical peg-in-hole operations and is provided as pseudo-code in Algorithm 2 (illustrated in Figure 4). In this operation, the tip of the held part is brought above and near to the hole located at $\vec{x}_{hole}$ (with diameter Dia) in the stationary part with the hole axis and held part axes aligned. Then, the held part is tilted away from the hole axis by $\theta_{tilt}$ and translated in the direction of the tilt by $\Delta x$. The tilted part is lowered into contact with the hole edge, using maximum normal force $F_{touch}$ as the stopping criterion to indicate contact. If aligned properly, the arc of the lowest held part edge now dips below the level of the hole. After contact is made, the held part is tilted $-\theta_{tilt}$ back into alignment with the hole axis with pressure applied. Then, an additional downward motion is attempted with a maximum normal force limit of $F_{insert}$ in an effort to seat the held part in its hole.

Using the action algorithms described in Section 4.5, we composed task-level algorithms for each of the three experiments. All three algorithms begin similarly. Both the held part and the reference part have a circular feature that can be used to locate them, so we deploy CircleLocator to get the 3D pose of the relevant circles. At the end of each algorithm, we move the robot to a safe pose $\vec{x}_{safe}$. This is done so that we can assume that every task begins with the robot in a known, safe configuration that far away from contacting either the base plate or staged parts.

Our bearing task algorithm begins by locating the bearing and the hole it belongs in. Appropriate view locations from which the gripper can view the circular features of the bearing and hole must be provided the by user in $\vec{x}_{BrgView}$ and $\vec{x}_{HolView}$, respectively. Then the bearing is grasped at the flange, with the gripper oriented vertically (fingers pointed $-Z$ down in the lab frame). The bearing is then moved above the hole by the InsertTilt routine, and a tilted insertion is attempted. Note that InsertTilt does not monitor the force-torque state of the wrist during the tilt-in operation, and the held part is tilted to the vertical regardless of what forces are sensed. After the insertion attempt completes, then a tamping operation is initiated from above the center of the bearing hole location.

Likewise, our pulley task algorithm begins by locating the top of the shaft (reference part) and the pulley (moved part). The pulley is grasped and brought to a position above the shaft with the plane of the pulley parallel with the plane of the base plate. Then, a spiral insertion is attempted with the intent of sliding the pulley down onto the shaft. Spiral insertion is followed by three tamping actions targeting the pulley; two on either side of the shaft, and one centered on the shaft. Finally, a twist operation, centered on the shaft, is executed with the assumption that the pulley is jammed at the top of the shaft. The twist operation does not monitor the gripper finger separation. So, if twisting is initiated after the pulley has fallen to the base, then Twist will grasp and twist the shaft with no result.

Our nut threading algorithm begins by locating the top edge of the stud (reference part) and the threaded hole of the nut (moved part). The nut is grasped using the PickPart procedure described in Section 4.4. The algorithm requires that two of the nut flats are parallel to the lab $Y$-axis in the lab reference frame in order to be gripped securely. Also, the nut is intended to be placed on the stud in this same orientation such that the threading action grasps the nut by the flats. (Although, the threading action was shown capable of threading the nut to finger-tightness even when the nut was grasped by the corners.) We attempt to seat the nut on top of the stud using a spiral insertion action. The threads do not allow the nut to slide down the stud, but the taper on the stud and the nut do allow the nut to rest on the stud until threading can begin.

The larger bearing has a relatively tight tolerance ($+0.2mm$) with its hole in the base plate, and its primary mode of failure is jamming. A jam occurred while inserting the bearing on seven of the twenty attempts. The tamp operation was able to seat the bearing in one of these cases, resulting in successful insertions. In one of the seven failures, an unknown movement error occurred that caused the robot arm to miss the hole in the base plate.

The force profile (Figure 6, top left) for a successful bearing insertion shows a large positive increase in $F_{z}$ (away from the wrist). This is due first to the bearing falling into the hole in the base, then to the bearing being bound in the hole as the robot wrist completes its arc to the vertical orientation.

The opposite trend in $F_{z}$ can be seen in force profile of an unsuccessful trial (Figure 6, bottom left). The robot tries to tilt the bearing into the hole, but the bearing is not aligned properly, and the bearing runs into the base plate at the edge of the hole, resulting in the reaction force in $-F_{z}$ (towards the wrist.)

During the bearing insertion operation, the robot hand is rotating about its local $Y$-axis, and the angle between the axis of the bearing and the axis of the hole in the base decreases. As the bearing tilts up, its contact point with the edge of the hole get further from the center of rotation, so there is greater resistance torque to further rotation until the bearing clears the edge of the hole at the top of the $T_{y}$ curve as shows in Figure 6, top right. In the failure case, $T_{y}$ continues to increase and bearing never enters the hole in the base plate. (Figure 6, bottom right).

The mean times to failure and success for the bearing task are similar, as the tilt-in action takes up the majority of the running time (Figure 6). All observed failures occurred during the insertion phase. Failure is immediately apparent by the end of the insertion action because the bottom edge of the bearing has either cleared the edge of its base plate hole, or it has not. In the former case, the tamping action that follows insertion cannot correct the condition. The mean time to success is slightly longer than that to failure because some successes are the result of the tamping action. There is one outlier failure case in which the bearing edge clears the hole, but the bearing is so badly jammed that it cannot be dislodged by tamping.

Van Wyk et al [20] use a tilt-insert method very similar to ours with a four-digit robot hand and in-finger load cells, albeit using much larger 3D-printed parts and hard-coded positions instead of vision. The system they present achieves a slightly higher success rate ($0.88$) than ours, and signifies there is a great opportunity to improve on the benchmark presented here.

A successful spiral insertion operation (Figure 7) for the small pulley is shown in Figure 8, top left. The $X$ and $Y$ forces are similar in magnitude and 90 degrees out of phase, as expected. The $Z$ force drops off suddenly when the pulley shaft clears the hole.

Failures of the spiral insertion operations witnessed during the experiment were typically due to the pulley being brought down onto the shaft in a way that caused the pulley to tilt in the fingers. We can see the evolution of the operation in this case in Figure 8, bottom left. There is no resistance to motion in the $-Y$ direction, and very little in $+Y$. As the pulley moves in the spiral, the shaft touches the hole at $X$ extremities only and the stop condition is met without any opportunity to bring the pulley back into a horizontal orientation that allows insertion. When the pulley is tilted this way, it will fall from the shaft as soon as the robot releases it. This was observed in all 7 failed trials.

Even after a successful spiral operation, the most frequent outcome is for the pulley to be jammed on the shaft, even though the top edge of the shaft has cleared the hole. The pulley ended the spiral operation in a jammed state in 12 out of 13 successful trials. Several tamping operations are performed to attempt to recover from a jammed state. The wrist sensor either experiences a $-Z$ spike in force when it contacts the jammed pulley, or the pulley has been freed and there is no contact. Unfortunately the release of the jammed state does not have a recognizable profile and we must rely on the following tamp operation to detect the unjammed state.

The wrist torques observed during the twist action follow a oscillatory profiles with frequencies that are either the same as, or twice that of, the frequency of wrist motion. The relationship between torques and wrist motion can be seen by comparing the top and bottom rows (center column) in Figure 8. A failed twist action (bottom row) has no change in waveform throughout the action. However, there is a noticeable step offset that manifests in the $Y$-torque felt at the wrist when a twist action corrects a jam. This is due to axis of the pulley coming into alignment with the shaft. When the twist action begins, the hand has grasped the pulley in its jammed, tilted configuration. When the pulley aligns with he shaft, it rotates about the hand’s $Y$-axis and imparts a constant torque to the hand about that axis.

Performance on the Small Pulley task had a lower success rate than the example set by Van Wyk et al [20], who were able to achieve a higher success ($0.95$) rate on a similar peg-in-hole task using a spiral search strategy, albeit using much larger 3D-printed plastic parts and hard-coded positions instead of vision.

The mean time to failure of the pulley task was about 30 seconds less than the average time of completion. Typically, the failure was in the spiral task, and the pulley would fall from the shaft, preventing any further actions from being taken, see also the histogram in Figure LABEL:pulleyHisto, right. Task completion takes more time because in almost every success case, the spiral action ends with the pulley securely on the end of the shaft, but bound there due to some misalignment between the pulley and the shaft. In most cases two to three tamps are required to dislodge the pulley and complete the task. There are two outliers, which represent the two trials that required a twist action to dislodge the pulley.

The initial approach to this task was unsuccessful, with all attempts resulting in failure. There were two failure modes. The first is misalignment, in which the spiral insertion action stopped when the lateral force criterion was met, but the pulley was tilted about an an axis formed by the gripper contact points. This failure mode revealed a case in which the assumption of the stopping criteria was incorrect. The second failure mode was jamming. In this mode, the pulley shaft clears the edge of the pulley hole, and so the stopping force criterion of the spiral motion is met. However, the hole and pulley axes are not aligned, and the pulley cannot slide down to the base plate. At present we do not have a technique for compensating for the first failure mode, as our system does not have means of sensing tilt of the held part within the fingers. Further investigation is needed for this case. There were two compensatory actions developed in order to compensate the second failure mode. The first is a twisting action that rotates the pulley about the shaft axis in an oscillatory way. This was meant to mimic the way a human might free a jammed peg-in-hole operation, but was not successful in any of the robot trials attempted. The second compensatory action is a “tamp” that applies force on the moved part (pulley) up to a predefined limit. Naturally, this has the potential to worsen a jammed condition if it imposes a moment on the pulley in the direction of its rotation out of the vertical. In order to account for this, three successive tamps were applied in the along a horizontal axis perpendicular to the direction of the finger opening: two offset from the shaft center on opposite sides, and one over the shaft center. This action was unsuccessful in resolving jams. Several different pulleys were tried with the same shaft. Neither the shaft nor the pulleys showed any obvious signs of defect or surface roughness on their contact surfaces.

The close tolerance between the pulley and the shaft made the jamming problem too persistent for the present system to compensate for. This issue was relieved by applying a small amount of silicone lubricant to the contact surfaces. This is not unreasonable, as it is expected for a product to ship with lubricant already applied to its moving parts. However, it does highlight the need for the system to be able to detect such jammed conditions. After lubrication, the jammed condition only occurred once during trials. All other failures observed were of the misalignment type described in at the beginning of Section 6.2.

The beginning of the stud approach is also a spiral insertion, but the shape of the stud makes the problem slightly different in ways both advantageous and disadvantageous for the technique applied. The stud (reference part) has a sharp, raised lip on its upper surface that causes friction between it and the nut. This can cause the nut to stick during points along the spiral action. In order to overcome this, the stop force criterion had to be raised. The high friction condition at the top and between the threads also makes it more difficult for the spiral action to recover from the misalignment condition described in Section 6.2. This misalignment was the primary reason for task failure. The spiral insertion action ends with a downward push. In cases where the action ends with the stud and nut mostly aligned, this had the effect of aligning the nut on the stud, due to the slight taper on the stud and the threaded hole on nut. This may have been the reason that no cross-threading events were observed during trials. It may be beneficial for the start of a threaded fastener operation to have some compliance, and this is worth investigating further.

Owing to the shape of the stud and the nut, the majority of task successes occur when the nut is brought immediately to the stud with little to no offset between the stud and nut central axes. When this occurs, the nut is already mostly “seated” on the stud, and the spiral action exits very soon after it begins because the nut has little lateral freedom in this case. The stopping force criterion is met immediately. The force profile of a failed spiral operation on the nut looks similar to that of the pulley task, described in Section 6.2. The $X$ and $Y$ forces have the expected phase offset, but they have differing zero offsets and magnitudes of oscillation, indicating that the nut has more freedom in the direction that the nut is tilted and hanging off-center from the stud. In this case the nut falls from the stud as soon as it is released, just as it was for the pulley and the shaft.

If the spiral action successfully places the nut onto the stud, then threading of the nut succeeds in all observed cases. There were no cross-threading events observed. The forces seen during the threading action have a saw-tooth profile. The recordings shown in Figures 10, top row, include only clockwise motion of the hand, as torque about the stud’s axis is applied to the nut. The return motion with the gripper open was not recorded. The most distinguishing feature of the force profile in Figure 10, top left, is the $Z$-force at the end of the action, that forms a descending staircase pattern. It was at this point in the threading operation that the tips of the gripper fingers were in contact with the base plate, and exerting more and more force on the base plate as the hand lowered with each turn. It was necessary for the gripper to grip the nut quite low, so that as much as the gripper surface as possible was in contact with the flats of the nut. If the gripper happened to slide downwards at all during the finalizing downward push of the spiral action, then the gripper fingers would be at or below the bottom of the nut when threading began. Occasionally, this condition resulted in friction between the fingers and the base plate to exert a torque sufficient to meet the stopping torque criterion for the threading action.

The difference between the mean time to failure and mean time to completion was greatest for the nut threading task. Nearly all failures of this task manifested during the spiral action. The threading action takes about two minutes to complete, and most of the observed successful trials completed in about 3 minutes.