Page - 76 - in Proceedings of the OAGM&ARW Joint Workshop - Vision, Automation and Robotics

TABLE I: Denavit-Hartenberg parameter Name Symbol Description joint angle θi angle between xi−1 and xi about zi−1 link offset di distance between the origin of frameFi−1 and Fi along zi−1 link length ai offset between frameFi−1 andFi along xi link twist αi angle between zi−1 and zi about xi subsequently the generation of point cloud representation. The dependence of each links pose to each other in the model will be set-up by applying the Denavit-Hartenberg (DH) convention to achieve the kinematic chain as shown in Figure 2b. TheDH convention describes the transformation between two frames of a manipulator by a homogeneous transformation matrix i−1T i∈R4×4 with four parameters by placing the joint coordinate frames in a predefined way. These transformations are representedby four basic transfor- mations between the joints as a chain of two rotations and two translations i−1T i=Rotzi−1,θiTranszi−1,diTransxi,aiRotxi,αi , (1) with the DH parameters listed in Table I. This convention will simplify the calculation effort for matching via the ICP algorithm to only one DoF per joint and keep the links dependent fromeach other. The deviation from the robot’s point cloud to the model is used for the calculation of the joint velocities to align both point clouds again. The whole implementation is realized with the free Point Cloud Library (PCL) [21], which includes numerous algorithms for handling of n-dimensional point clouds and three-dimensional geometries, in the frameworkof theRobot Operating System (ROS) [22]. ROS is a collection of li- braries, tools and conventions for writing robot operating software. III. MODEL IMPLEMENTATION In an initial step point clouds from the CADmodels of the robot’s links have to be generated. It is important that, before the point clouds can be generated, the alignment of theCADmodeled links are prepared correctly asmentioned in Section II. First, they have to be aligned in their initial direction (cf. Figure 2b), second, their coordinate system must be set to the center of their rotation axis, and third, the link coordinate systems have to be translated such that theymatchwith theDHconventionas it is done for link four and six (translation in x direction) as shown in Figure 2b. The point clouds are generated by the tool pcl_mesh2pcd (based on take views and fuse them together) from the PCL to achieve an envelope point cloud of the CADmodels. Every link is implementedas anownobjectwith theprop- erties summarized in Table II, with the first four parameters as constants and the transformationmatrix and joint angle as variables. The robot’s linkswill not separate fromeachother during the ICP algorithm performs thematching, since they TABLE II: Robot link properties Parameter Class Name std::string Point cloud pcl::PointCloud<PointXYZRGBA>* Color pcl::visualization::PointCloudColor-HandlerCustom<PointXYZRGBA>* DH-parameter std::vector<double> DH-transformationmatrix Eigen::Matrix4f Joint angle std::double are coupledby the transformationofDHwith theparameters of Table III and the dependencies given by 0Tn= n ∏ i=1 i−1T i , (2) Tn,α= 0TnTα nT0 , (3) n−1Tn= n−1T0Tα 0Tn . (4) In Equation (2), 0Tn∈R4×4 is the transformation of joint n between the base coordinate system and the coordinate system of joint n as the product of the DH transformations. By the use of the short notation c(·)= cos(·) and s(·)= sin(·), theDHtransformationmatrix i−1T i fromEquation (1) can bewritten as i−1T i= [i−1Ri i−1pi 0T 1 ] =     cθi −sθicαi sθisαi aicθi sθi cθicαi −cθisαi aisθi 0 sαi cαi di 0 0 0 1     , (5) with i−1Ri∈R3×3 the rotation between the frameFi−1 and Fi, the translation i−1pi∈R3×1 from the originOi−1 toOi and thevectorofzeros0∈R3×1.Tn,α∈R4×4 inEquation (3) is the transformationof jointn in the base coordinate system by the transformationmatrix Tα= [ Rz(α) 0 0T 1 ] ∈R4×4 , (6) with the rotationmatrixRz(α)∈R3×3 along the joint rotation axis which is obtained from the Euler angles by the ICP TABLE III: Denavit-Hartenberg parameters of the industrial robot ABB IRB 120 JointNr. θi [◦] di [mm] ai [mm] αi [◦] 1 q1 165 0 0 2 q2 125 0 −pi/2 3 q3−pi/2 0 270 0 4 q4 0 70 −pi/2 5 q5 302 0 pi/2 6 q6 0 0 −pi/2 76