Fast programming of Peg-in-hole Actions by human demonstration

2 downloads 0 Views 1MB Size Report
Maersk Mc-Kinney Moller Institute. University of Southern Denmark. DK-5230 Odense M, Denmark. Email: [email protected]. Abstract—Peg-in-Hole actions play ...
2014 International Conference on Mechatronics and Control (ICMC) July 3 - 5, 2014, Jinzhou, China

Fast Programming of Peg-in-hole Actions by Human Demonstration Y. Yang, L.L. Lin, Y.T. Song

B. Nemec, A. Ude

A.G. Buch, N. Kr¨uger, T.R. Savarimuthu

Institute of Plasma Physics Chinese Academy Of Sciences Hefei P.O.1126, Anhui, China Email: [email protected]

Humanoid and Cognitive Robotics Lab Jozef Stefan Institute 1000 Ljubljana, Slovenia Email: [email protected]

Maersk Mc-Kinney Moller Institute University of Southern Denmark DK-5230 Odense M, Denmark Email: [email protected]

Abstract—Peg-in-Hole actions play an important role in industrial assembly. In this paper, we present a system which performs such actions after learning by human demonstration. Strategies for getting robust human-demonstrated PiH trajectories are investigated, and a number of PiH experiments are conducted to verify the performance of the proposed approach. The results show that our approach leads to a high PiH success rate and allows the robot to fine-tune the PiH trajectories to achieve faster performance. Keywords—Peg-in-Hole, demonstration.

I.

Robot

automation,

Learning-by-

I NTRODUCTION

The cumbersome process of setting up robot systems and their programming is still a hurdle to make the use of robots economical for small batch size production in particular. Setup and programming times for robots are dominated by a number of sub-problems: First, specialized grippers are often used for grasping and manipulation to achieve good force control. These grippers often need to be designed or refined for particular objects occurring in the assembly process. Second, it is often required that the position and orientation of objects are predetermined with high accuracy. This usually requires specific machinery for accurate positioning, such as feeding stations which also requires expert knowledge and causes additional hardware costs. Third, robot grasps and trajectories (including appropriate forces) need to be taught or programmed, which is often done through menu-oriented control devices, which again is a quite tedious procedure.

Fig. 1. The MARVIN platform: 2 UR robot arms are equipped with a Schunk SDH gripper (right) and a Robotiq hand (left). The force-torque sensor is mounted between the right robot end and the attached SDH gripper. A Bumblebee stereo camera (centered behind the robots) is used for the detection of objects in the scene, which is based on point cloud data. High-density point clouds are achieved by the use of a texture projector (left of the camera), which helps the stereo matching process.

Motivated by the good skills of human being when performs assembly tasks, we introduce Learning by Demonstration (LbD) [1], [2], [3] strategy to our Peg-in-hole (PiH) manipulation. In this paper, we introduce a system which addresses the problems mentioned above. Firstly, instead of designing specialized grippers, we make use of a dexterous hand which is able to perform a variety of grasps. Secondly, we use vision-based pose estimation to determine the position and orientation of objects, removing the need to accurately position the objects. Thirdly, we program robots by copying the action of the human performed on the objects to be assembled, which allows the human to apply all its dexterity in the manipulation process. Finally, we introduce a learning scheme which allows for the robot to progressively improve performance over multiple runs of the same action, in terms of

The paper is structured as follows: The next section II gives an introduction to the hardware, software and algorithms used to perform PiH in this paper. Section III investigates strategies for teaching robust PiH trajectories by human demonstration. In section IV, we present experiments in order to verify the performance of the proposed PiH strategy. Finally, we summarize the paper in section V.

978-1-4799-2538-4/14/$31.00 ©2014 IEEE

990

a reduction in both the applied forces and the actual execution time. We exemplify the properties of the system on two PiH assembly operations of different complexity, namely for round and square pegs. The programming and fine-tuning of these operations altogether take less than 30 minutes, which shows that assembly actions can be setup in rather short time by our approach.

II.

P ROPOSED SYSTEM

All the experiments are performed on our experimental MARVIN platform (see figure 1). Two Universal Robots (UR) manipulators are mounted on each side of the main table. The robot used in this work is equipped with a Schunk SDH gripper, and a force-torque sensor is mounted between the

robot end and the SDH gripper (rightmost robot in figure 1). The magnetic 6D motion tracking system trakSTAR1 is used to track objects during human demonstration. A Bumblebee stereo camera provides stereo images of the objects on the platform, and the OpenCV2 block matching algorithm is used for 3D reconstructions of the scene to point cloud data. In order to achieve better pixel correspondences for 3D reconstruction, a random noise pattern is projected into the scene by projectors (see figure 3). In our implementation, ROS3 provides middleware interfaces among different node-based publisher/subscriber components in the system. Our robot control software is based on the RobWork libary [4], which provides path planning algorithms, collision detection and forward/inverse kinematics for computing target configurations for the robot. In order to simplify the management of the PiH workflow, the smach system [5], which is a ROS-independent Python library, is adopted to build a hierarchical state machine of robot behaviors at task level. In the following section II-A, we briefly describe the principle of learning a PiH operations by human demonstration. The next section II-B presents the pose estimation algorithm we have developed for our platform. Section II-C gives some brief preliminaries of PiH action execution using a control algorithm based on Dynamic Movement Primitives. A. Learning by Demonstration The trakSTAR system is used to track the Cartesian trajectories of human demonstrator when performing the PiH tasks (see figure 2). The demonstrated trajectory is then directly transfered to the robot to make robot follow the motion of human demonstrator. The peg manipulated by the human, with tracking sensor mounted in the center, is a physical copy of the peg used in the assembly process. The tracking system is well calibrated [6], and the control point of the robot is approximately located in the center of the grasped peg. Thus the robot performs a very accurate copy of the human demonstrator’s movement. More details about this setup are presented in [7].

Fig. 2. Learning by demonstration setup: a teleoperation system is used to guide the robot, and the objects manipulated by the human are tracked using high-precision magnetic sensors.

By using this robot teleoperation system, the human can perform the PiH assembly task in a intuitive way, with all 1 http://www.ascension-tech.com/realtime/RTtrakSTAR 2 http://opencv.org 3 http://www.ros.org

991

his/her sensory-motor skills to demonstrate the best possible trajectory. The force/torque sensor mounted at the wrist of the robot records the perceiving forces/torques occurring through the contact between the peg and the hole. The recorded forces/torques will then be encoded by dynamic movement primitives algorithm to optimize the trajectory adaption in a new objects situation, as described in section II-C. A basic demonstrated PiH trajectory thus contains: •

Target hole pose



Time stamp of the each trajectory point



Force-torque vector relative Fx , Fy , Fz , Mx , My , Mz



Transformation of the object frame relative to the robot base frame: T = [P, Q], where P is the position vector and Q is the quaternion representation of the orientation

to

object

frame:

B. Pose Estimation A reliable pose estimation method is a key component to achieve a robust PiH execution. Pose estimation of the objects is performed using a point cloud based detection system, which we have implemented using several basic components of the Point Cloud Library [8]. We need to find the full 6D pose of each object involved in the PiH actions, and we need to be able to grasp the objects for the following PiH action. The robot and the camera must therefore refer to the same intrinsic coordinate frame of the objects. This is unambiguously defined in the CAD models, so we have chosen to perform pose estimation based on the CAD models (see figure 6), which we have resampled to point clouds. The algorithm is described in detail in the paragraphs below. 1) Preprocessing and Initial Candidates: Prior to pose estimation, both the scene point cloud and the model point clouds are downsampled to a resolution of 5 mm to reduce the processing time. We then perform table removal followed by Euclidean cluster extraction in order to segment the different objects in the scene. This results in a small set of isolated clouds, each representing a partial view of one of the objects. We now perform principal component analysis (PCA) of the segments to associate a full reference frame on SE(3) to each segment. In the offline stage, we have done the same PCA computation to the object models, and stored the relative transformation between the intrinsic object coordinate frame and the PCA frame. We now compute the axis-aligned bounding box of each segment along the three PCA axes to get an invariant estimate of the volume of the perceived object. Since we have done the same for the objects, we can now produce matching candidates for each segment by simply checking which of the object models have approximately the same volume. We produce a recognition candidate between a segment and an object if the relative difference in volumes is less than 2/3. The PCA frame pair of a segment and each of the matched object models can now be used to generate an hypothesis alignment pose Tˆ for each matched object as follows: −1  pca obj seg = Tcam · Tobj (1) Tˆcam

seg where we use Tcam to denote the camera-relative PCA frame of the segment, consisting of the eigendecomposition and pca denotes the precomputed PCA centroid of the segment. Tobj frame of the matched object, relative to the intrinsic object obj coordinate frame. The hypothesis pose Tˆcam thus gives the alignment transformation used to bring the candidate object into alignment with the scene segment, relative to the camera sensor. For the special case that the object is already centered and axis-aligned, its intrinsic frame would coincide with the pca the identity transform. PCA frame, making Tobj

2) Pose Refinement and Occlusion Reasoning: For each segment in the scene, we now have a number of object obj candidates, and associated pose hypotheses, Tˆcam , and we need to decide which object hypotheses fits best, if any. We start by performing a fine registration of each object candidate obj as starting by Iterative Closest Point (ICP) [9]—using Tˆcam guess—to bring each matched object into accurate alignment with the segment. We now build an octree of the segment with resolution equal to the downsampling resolution (5 mm). For each aligned object, we now perform occlusion removal by raycasting into the octree structure to remove object points not visible in the scene under the hypothesized alignment. The remaining visible points are now used to perform geometric verification with the segment points. 3) Geometric Verification: For each segment, we now have the object candidates brought into alignment using ICP, stripped of occluded points. We should now have a one to one match between the points in the segment, and the remaining object points, so we perform another fine registration using ICP of the visible points to get an optimal pose. We then perform geometric verification between the visible points and the segment points by a weighted RMS point to point error between closest points. We use k-d trees [10] for fast nearest neighbor (NN) point search between each visible point and its closest segment point. We then weight the Euclidean point distance by a penalty derived from the surface normals, estimated on both the object and the segment surface. Denote the surface normal a certain point p as n. For each nearest point pair, we use the following penalty: d(pobj , pseg ) · (2 − nobj · nseg )

(2)

where d denotes the Euclidean distance found by the NN search. The weight term in the last parenthesis equals 1 when the two surface normals point in the exact same direction, and increases when the mismatch in orientation increases. This penalty takes into account both the proximity of the models, and their shape, allowing for high discrimination. Each object alignment candidate is thus evaluated using (2), and for each segment we keep only the best matching object. Finally, we place the pose estimate into the robot base frame cam , which we have obtained by an external calibration pose Trobot through accurate hand-eye calibration: obj cam obj = Trobot · Tcam Trobot

(3)

The algorithm performs very well, both in terms of robustness and efficiency, and it deals with multiple instances of the same object. The total processing time for the example scene in figure 3, including pose refinement for good alignments, is less than 5 s. The mean expected alignment error is directly 992

proportional to the downsampling resolution, and we can thus expect, in the worst case, that the uncertainties from pose estimation amount to translation errors of 5 mm. Additionally, cam will contribute to the robotcalibration errors within Trobot relative object pose; however, these errors are much smaller in magnitude than those of the pose estimation system. In practice, we have experienced the total alignment error of our system to be within a range of 1-2 mm for most cases by far.

Fig. 3. Input scene (left) to the pose estimation algorithms, showing the projected texture pattern for better stereo reconstructions, and multi-instance pose estimation results (right).

C. Trajectory Adaptation with Force Feedback The basic idea of trajectory adaptation is to use force feedback to adjust the recorded trajectory, until it matches the desired force profile. The approach is based on Dynamic Movement Primitives (DMPs) [11], which provides a comprehensive framework for the specification of robot control policies. DMPs are based on non-linear differential equations and result in smooth kinematic control policies, which is essential for achieving robust and autonomous execution of assembly tasks [12]. When applying the demonstrated PiH trajectory in a new situation, we cannot directly replicate the demonstrated trajectory, due to inaccuracies of the vision system, uncertainty of grasping, and different joint space configurations of the robot. During PiH execution, the robot attempts to adapt the demonstrated forces and torques rather than the positions and orientations. The adaptation of the desired forces and torques can be accomplished using either admittance or impedance control, where the robot motion is used to reduce the force and torque errors. A DMPs phase stopping technique is implemented to slow down the trajectory evolution whenever the force or torque errors exceed a limit, and ensure stability of the robot control. The approach also enables the robot to adjust the demonstrated trajectory in a few iterations by capturing the offset to the demonstrated trajectory, and adding this offset to the demonstrated trajectory in the next learning iteration. After these learning iterations, the arising forces and torques during execution of the PiH operations are similar to the ones recorded by human demonstration. More details of this method are discussed in [13] and [14]. III.

ROBUST P I H M ANIPULATION WITH L EARNING BY D EMONSTRATION

In this section, strategies for teaching the robot PiH operations are investigated. The time consumed to get a valid trajectory depends on the degree of PiH complexity as well as the degree of familiarity of the human demonstrator with the teleoperation system used for teaching; the latter factor is discussed in [6] where the performances between novice users and expert users are compared. The time required by an expert

(a) Move peg to the initial position of the recorded trajectory Fig. 4.

(b) Initial collision of peg and hole edge

(c) Align peg axis to the hole axis

Robust round peg trajectory teaching procedures.

(a) Move the peg to the initial position of recorded trajectory

(b) Search for the hole corner

(d) Align the peg axis to the hole axis Fig. 5.

(d) Insert the peg until it reaches the bottom

(c) Rotate the peg and make the peg side collide with the hole edge

(e) Insert the peg until it reaches the bottom

Robust square peg trajectory teaching procedures.

user to teach a valid trajectory for a round peg is approx. 15 minutes on our platform, including the setup time, the human demonstration as well as seven repetitions of the robot action including the pose estimation, grasping and execution of the PiH action. For a square peg, where alignment with the hole is more complicated, the total teaching time is approx. 20 minutes. The setup for teaching PiH trajectories by human demonstration is shown in figure 2. Two faceplates are placed in the scene, and the robot grasps the square peg (or round peg) from one faceplate in order to reduce the grasping uncertainties. Then the operator manipulates the physical copy of the peg, which has a magnetic sensor mounted, to perform the PiH action, and meanwhile the robot follows this movement. A physical button (in the left hand of the demonstrator) controls the state of demonstration: when this button is pushed down, demonstration will be activated. A software is also developed to start (and finish) logging the demonstration trajectory. Based on our experience, a robust PiH manipulation trajectory for both the round and the square peg can be obtained by following the procedures shown in figure 4 and 5. The teaching strategy details for the round peg (tight shaft peg) are described in [15]. The main idea is: during the approach phase, the round peg must be tilted approx. 20 degrees and make an initial contact with the edge of the hole. The contact force between peg and hole can be considered as a signal, activating the robot to rotate along the contact point and align the peg and hole axis. The PiH strategy based on force feedback is also described in [16], and in [17], it is shown how passive compliance can be used to solve a 2D PiH assembly. For the square peg, we apply the following strategy: 993



Grasp type: Similar to the round peg, a three finger ball grasp is used. This grasp configuration ensures stability of the peg while it contacts the hole. The peg must be standing upright when grasping, and in order to avoid large grasp uncertainties, the peg is placed in the faceplate prior to grasping.



Start configuration: This is the start configuration before recording the PiH trajectory. The square peg is tilted around 20 degrees relative to the vertical hole axis, and one of its corners is positioned approximately 20 mm above the square hole center (figure 5(a)).



Initial contact: Move the square peg toward the hole corner, until the peg edge touches the hole corner (figure 5(b)). This step is the position alignment of the peg and the hole. The contact forces and torques are recorded as a particular force event prior to the subsequent angle alignment between peg and hole (see next step).



Initial angle alignment: Rotate the peg along the initial touch point, until the side of the peg touches the hole edge (figure 5(c)). Similar to the previous step, the recorded forces and torques give another force sign before the final angle alignment.



Final angle alignment: Keep contact between initial touch point and edge, and rotate the peg to align the peg axis to the hole axis (figure 5(d)).



Insertion: Move the peg linearly down the hole (figure 5(e)). In this step the peg axis should be kept in

alignment with the hole axis, and the contact forces and torques should be small. Figure 5 briefly shows the procedures of demonstrating a square peg PiH trajectory. Note that during these steps we use two particular force patterns to fulfill the position alignment and angle alignment. These two force patterns indicate the different PiH operation states of the square peg, and they are easy to distinguish from each other in force-torque space. IV.

E XPERIMENTS

A series of PiH operations are performed to verify the proposed strategy. The objects used for the PiH experiments are a subset of the robotic Cranfield benchmark [6], shown in figure 6. The faceplate center hole for the tight shaft is 43 mm in diameter and has a 5 mm chamfer, whereas the square holes are 32 mm wide with a 3 mm chamfer. The square peg and tight shaft peg are 120 mm along the largest axis, the end of square peg is 30 mm wide and the end of tight shaft peg is 42.4 mm in diameter. In section IV-A 25 trials of PiH actions for both peg types are conducted, and the success rates are given. Section IV-B shows the trajectory optimization during PiH operation of tight shaft peg.

(a) square Fig. 6.

(b) tight shaf t

(c) f aceplate

TABLE I.

PIH

TEST WITH TRAJECTORY RECORDED BY STRATEGY

Total trials Success trials

tight shaft peg 25 25

LBD

square peg 25 22

B. PiH Operation Optimization In this experiment, the target faceplate is fixed on a solid plate, the robot grasps the tight shaft peg from the initial faceplate and inserts it into the target faceplate, and repeats the final insertion part seven times. The execution time is recorded for each learning cycle. Figure 7 shows the learned position components of the trajectories generated in the seven learning trials. During each learning cycle, the robot adjusts the demonstrated trajectory based on differences between the current measured force-torque image and the demonstrated force-torque image. The position offset relative to the demonstrated trajectory is captured, and this offset is added to the demonstrated trajectory in the next learning cycle. Figure 8 gives the forces profiles both for the demonstrated trajectory (dashed) and the learned trajectories. The constant offsets in the profiles between the measured and demonstrated forces are due to imperfect force sensor calibration, and do not influence the performance. After seven learning iterations, the forces occurring during execution of the PiH operations are similar to the ones recorded by the human demonstrator. All the plots in figure 7 and figure 8 are normalized to equal duration (phase time, see in section II-C) in order to compare the trajectory adaption of each trial. As the learning cycles increase, the PiH trajectory is optimized and the execution time decreases, which is shown in figure 9.

The Cranfield benchmark objects used in the experiments.

A. PiH Experiment This experiment deals with insertion of the tight shaft peg and the square peg into the corresponding holes of the faceplate. The experiment scene contains two faceplates, one is denoted the initial faceplate with a peg inside, the other one is the target faceplate. The setup of this experiment is shown in figure 3. The robot grasps the peg from the initial faceplate and inserts it into the target faceplate. Between different execution trials, the faceplates are randomly placed within the workspace of the robot, camera and projector, and the pose estimation system is used to accurately estimate the pose of both faceplate instances present in the scene, where one faceplate is partially occluded by the pre-inserted peg. 25 PiH trials are performed for both pegs, and we record the number of success trails in table I. Each trail involves pose estimation, grasping, and insertion with DMPs control. The results show that our PiH approach is reliable and robust. The three failure trials for the square peg occur when the corner end of the peg collides with the faceplate surface instead of touching the hole corner (see figure 5(b)). The DMPsbased control algorithm cannot deal with this situation as it cannot determine where the hole is. One explanation why this occurs is that the grasp uncertainties for the square peg under imperfect vision (noise in pose estimation) are more severe than the round peg. 994

Fig. 7. The learned offset during seven learning cycles of tight shaft PiH execution.

V.

C ONCLUSION

The PiH implementation strategy described in this paper contains learning of PiH actions by human demonstration and execution by force-based DMPs control. We propose a sequence of procedures to get a high quality in the demonstrated PiH trajectories, both for round pegs and square pegs.

[2] [3]

[4]

[5] [6]

[7]

[8] Fig. 8. Force measured during seven learning cycles of tight shaft PiH execution. The dashed lines refer to the desired force which is recorded during human demonstration.

[9]

[10] [11]

[12] [13]

[14] Fig. 9. PiH actual execution time of tight shaft in seven learning cycles. The dashed line refers to the execution time of the human demonstrated trajectory.

The demonstrated trajectory is utilized by DMPs control as a reference with force and torque information. A number of PiH experiments have been performed to verify the performance of the proposed approach, and the results show that PiH actions for both square and round pegs can be executed robustly. Furthermore, we demonstrate the use of our approach for PiH trajectory optimization by executing seven consecutive insertion cycles, and our results show that the execution time decreases rapidly after the first couple of cycles, before settling to a value close to the execution time of the human demonstrated trajectory. ACKNOWLEDGMENT The research leading to these results has received funding from the European Community’s Seventh Framework Programme FP7/2007-2013 (Specific Programme Cooperation, Theme 3, Information and Communication Technologies) under grant agreement no. 269959, IntellAct. R EFERENCES [1]

Y. Kuniyoshi, M. Inaba, and H. Inoue, “Learning by watching: Extracting reusable task knowledge from visual observation of human

995

[15]

[16]

[17]

performance,” Robotics and Automation, IEEE Transactions on, vol. 10, no. 6, pp. 799–822, 1994. S. Calinon, “Robot programming by demonstration,” in Springer handbook of robotics. Springer, 2008, pp. 1371–1394. R. Dillmann, “Teaching and learning of robot tasks via observation of human performance,” Robotics and Autonomous Systems, vol. 47, no. 2, pp. 109–116, 2004. L.-P. Ellekilde and J. A. Jorgensen, “Robwork: A flexible toolbox for robotics research and education,” in Robotics (ISR), 2010 41st International Symposium on and 2010 6th German Conference on Robotics (ROBOTIK). VDE, 2010, pp. 1–7. B. Jonathan and C. Steve, “The smach high-level executive,” in IEEE Robotics & Automation magazine, Dec 2013. D. Liljekrans, “Investigating peg-in-hole strategies through teleoperation,” Master’s thesis, The Maersk Mc-Kinney Moller Institute,Faculty of Engineering,University of Southern Denmark, 2012. T. Savarimuthu, D. Liljekrans, L.-P.Ellekilde, A. A. Ude, B. Nemec, and N. Kruger, “Analysis of human peg-in-hole executions in a robotic embodiment using uncertain grasps,” In 9th International Workshop on Robot Motion and Control, RoMoCo, 2013. R. B. Rusu and S. Cousins, “3d is here: Point cloud library (pcl),” in Robotics and Automation (ICRA), 2011 IEEE International Conference on. IEEE, 2011, pp. 1–4. P. Besl and N. D. McKay, “A method for registration of 3-d shapes,” Pattern Analysis and Machine Intelligence, IEEE Transactions on, vol. 14, no. 2, pp. 239–256, Feb 1992. M. Muja and D. G. Lowe, “Fast approximate nearest neighbors with automatic algorithm configuration.” VISAPP (1), pp. 331–340, 2009. A. J. Ijspeert, J. Nakanishi, and S. Schaal, “Movement imitation with nonlinear dynamical systems in humanoid robots,” in Robotics and Automation, 2002. Proceedings. ICRA’02. IEEE International Conference on, vol. 2. IEEE, 2002, pp. 1398–1403. B. Nemec and A. Ude, “Action sequencing using dynamic movement primitives,” Robotica, vol. 30, no. 5, pp. 837–846, 2012. B. Nemec, F. Abu-Dakka, J. A. Rytz, T. R. Savarimuthu, B. Ridge, N. Kr¨uger, H. G. Petersen, J. Jouffroy, and A. Ude, “Transfer of assembly operations to new workpiece poses by adaptation to the desired force profile,” IEEE International Conference on Advanced Robots, Montevideo, Uruguay, 2013. F. J. Abu-Dakka, B. Nemec, and A. Ude, “Peg-in-hole using dynamic movement primitives,” in Raad 2012 Proceeding. 21th International Workshop on Robotics in Alpe-Adria-Danube Region (Naples, 10-13 September 2012). ESA, 2012, p. 143. L. Ellekilde, B. Nemec, D. Liljekrans, T. Savarimuthu, D. Kraft, F. AbuDakka, A. Ude, and N. Kruger, “Robust peg-in-hole manipulation motivated by a human tele-operating strategy.” T. Yamashita, I. Godler, Y. Takahashi, K. Wada, and R. Katoh, “Pegand-hole task by robot with force sensor: Simulation and experiment,” in Industrial Electronics, Control and Instrumentation, 1991. Proceedings. IECON ’91., 1991 International Conference on, Oct 1991, pp. 980–985 vol.2. S.-k. Yun, “Compliant manipulation for peg-in-hole: Is passive compliance a key to learn contact motion?” in Robotics and Automation, 2008. ICRA 2008. IEEE International Conference on. IEEE, 2008, pp. 1647–1652.