included in the CAD-2-SIM-framework by implementing a solver in a Python class and ... Fb be two frames in the same global coordinate system. Let P(t) a.
CAD-2-SIM – Kinematic Modeling of Mechanisms Based on the Sheth-Uicker Convention Bertold Bongardt Robotics Innovation Center DFKI, Bremen, Germany.
Abstract. This paper describes methods of kinematic modeling and of forward kinematics computation that are implemented in a new kinematic modeling software – named CAD-2-SIM – that transfers models of mechanisms from computer-aided design software to simulation. In particular, the software is based on a convention developed by Sheth and Uicker in 1971 that we call the two-frame convention, since it requires the definition of two specifically placed and named frames per joint. This convention simplifies the kinematic specification of an arbitrary mechanism and enables computing its forward kinematics. The presented notation uses an indexing scheme based on graph theory. Keywords: Kinematic Modeling, Parallel Mechanisms, Graph Theory, Computer-Aided Design, Open Source Software.
1
Introduction
Design on the one hand, and testing, simulation, and control on the other hand, are two complementary aspects in the process of engineering new mechanisms, e.g., robots. Nowadays, the design process – also called kinematic synthesis – is supported by (mainly proprietary) computer-aided design (CAD) tools, while control and kinematic-dynamic analysis is supported by a multitude of simulation software, e.g., Openrave [4]. However, the process of transferring a kinematic model from one software to another often is a time-consuming task. The novel, opensource program CAD-2-SIM is a tool to simplify this transfer by creating an automated interface between the two sides of software. Thus, it can significantly increase the overall development speed for new mechanisms. Fig. 1 shows the general processing of CAD-2-SIM: the software needs two kinds of input: first, a CAD model of the mechanism is needed. This model is enriched with a set of frames. Secondly, a Python class which provides further information about that mechanism needs to be provided. The output of CAD-2-SIM is three-fold: first, a specification of the mechanism for Openrave is composed in XML format, second, an automatic computation of the forward kinematics (FK) is conducted, and third, a documentation in LATEX format can optionally be created. This paper focuses on the description of the FK-computation (Sec. 4) and the generation of the XML specification (Sec. 5). One of the most important features of CAD-2-SIM is the placement of the frames in CAD software. They are all placed at the anchor points1 of the joints 1
The anchor point of a joint is the intersection of the joint axis with the plane of touch.
Fig. 1: Schematic sketch of the program CAD-2-SIM: The transfer of kinematic models from CAD to simulation software, the computation of forward kinematics, and some LATEX documentation are provided by the central Python program. (Sec. 3.3) which results in a very intuitive handling for the designer. In contrast, when using DH conventions [3], certain geometric constraints must be obeyed (Sec. 3.1), that are stemming from the link geometry. Also, the two-frame convention provides a precise separation between time-variant and time-invariant displacements of a mechanism. From that follows, for example, that the displacment of a simple joint can be expressed as a pure screw displacement. Due to the automatic computation for tree-like structures (Sec. 4.1), it is possible to define an arbitrary number of end-effectors: this is a convenient opportunity to define certain frames of interest. Finally, any parallel substructure can be included in the CAD-2-SIM-framework by implementing a solver in a Python class and embedding this into the computation via a plugin mechanism. The presented modeling is consistent in the sense that parallel substructures simply act as complex joints (Sec. 4.2). The structure of this paper is as follows: In Sec. 2, an introduction to basic necessary entities is provided. Subsequently, in Sec. 3, we introduce three different conventions for specification of mechanisms. In particular, the Sheth-Uicker convention is presented. This convention builds the basis for the method of computing the forward kinematic in CAD-2-SIM, which is explained in Sec. 4. In Sec. 5, the generation of a mechanism specification for use with Openrave is briefly described. Finally, we present some outlook in Sec. 6.
2 2.1
Preliminaries Terminology
In this section, a few standard terms are presented which are needed for this paper. A link is a rigid body of a mechanism. A link L may connect more than two joints. A joint J is a kinematic element that connects exactly two links in a defined manner and with a defined number of degrees of freedom. A frame F is the short name for a local coordinate system. Its pose is defined by position and orientation and may change over time. The origin O is a short name for the (time-invariant) global coordinate system, or in other terms, the basis of the vector space. In the entire paper, we work with the standard basis of R3 and the standard basis of GL(4). 2.2
Transformations and Displacements
The group of proper rigid body transformations in three-dimensional Euclidean space is denoted by SE(3) = SO(3) n R3 , (1)
the Special Euclidean Group, according to [17, Sec. 2.5]. In this paper, we work with the homogeneous representation GL(4). For this matrix, we use the symbol S. The matrix S consists of a rotation matrix R and a translation vector t. S = ( R0 1t ) (2) In general, an element of SE(3) represents a (finite frame) displacement. We use this term as a short term for a proper rigid body transformation. Since several algebraic structures are suitable to express finite displacements [7,13,15,17], in the remainder of this paper, we work with functions instead of matrices and denote any displacements by D. However, for any displacement, a representation as a homogeneous matrix S is available and suitable. Poses. A pose is a special displacement. It describes a displacement relative to the origin. To indicate this, for a pose we use the notation P instead of D. Simple Joint Displacements. According to [9], joints can be categorized into three classes of lower-pair, higher-pair, and wrapping joints. In this paper, we constrain the discussion to lower-pair joints that only have one degree of freedom, namely any rotative, prismatic or screw joint. The displacement of such a joint can be expressed via a finite screw (see, e.g., [2]), and we call it a simple joint. A nonsimple joint, we call a complex joint. In [18] it was demonstrated how modeling of other lower pairs can easily be integrated into the two-frame convention. If S represents a pure translation, the rotation matrix R equals the identity element of SO(3), the identity matrix I3 . If u denotes the unit vector of the direction of a pure translation, and q the magnitude of the movement, the translation vector t is computed as t = t(q) = q · u . (3) If S represents a pure rotation, the translation vector t equals the identity element of R3 , the zero vector 03 . If u denotes the unit vector of the axis of a pure rotation, uˆ the corresponding skew-symmetric matrix2 , and q the magnitude (i.e., angle) of the rotation. The rotation matrix R is computed as ˆ . R = R(q) = exp(q · u) (4) ˆ can be computed by the well-known Rodrigues formula3 The expression exp(q · u) ˆ = I + sin q · uˆ + (1 − cos q) · uˆ 2 . exp(q · u) (5) 2.2.1 Spatial Displacement. Let t be a fixed and arbitrary timestep, let Fa and (t) Fb be two frames in the same global coordinate system. Let P(t) a and Pb describe the poses of these frames at timestep t. Then, the spatial displacement between the two frames from Fa to Fb is expressed via −1 ◦ P(t) (6) D(t) : Fa(t) 7→ F(t) D(t) = (P(t) a ) (a,b) b (a,b) b p
The spatial displacement D(t) describes a passive interpretation of a transforma(a,b) tion. This is indicated by the usage of the symbol 7→p . This notation is chosen so that it matches the notation in classic robotics textbooks (e.g., [16]). The FK map of a kinematic chain is computed as D(0,n) = D(0,1) ◦ D(1,2) ◦ · · · ◦ D(n−1,n) , where the interpretation works from right-to-left because of the passive interpretation. 2 3
Notation is chosen to be consistent with [12]. See [12, Sec. 2.2.] for a derivation.
Fig. 2: Displacement examples: spatial dis(t) placements (e.g., Da,b ) are marked by dotted arrows. The dashed arrow (D(t,s) ) indicates a b temporal displacement.
Fig. 3: Example of similar displacements. D(t) is similar to b C(t) with respect to P(t) . b b
2.2.2 Similar Displacements. During the forward kinematics computation, the following similarity relation is needed (see, e.g., [1, Sec. 2.6]): D(t) = (P(t) )−1 ◦ C(t) ◦ P(t) . b b b (t) b
(7) (t)
The displacement D is similar to the displacement C with respect to the similarity transform D(s) . See Fig. 3 for a simplified sketch. b 2.3
Mechanisms and Graph Theory
A graph is a pair G = (V, E) of sets such that E ⊆ V × V : The elements of the set V are called vertices, the elements of the set E are called edges (e.g., [5]). The combinatorics of a graph are defined by means of an incidence or adjacency structure. A characterization by McPhee reads ‘Linear graph theory is that branch of mathematics that studies the description and manipulation of system topologies’ [11]. Specifically, in application to kinematics, graph theory provides the tools to distinguish between kinematic chains, trees, and graphs. Our modeling (Sec. 3.3) is similar to the one presented in [10] in terms of working with a frame graph. However, while the authors of [10] perform dynamics computations, we focus only on kinematics computations. Spanning Trees. A spanning tree T = (V, E0 ) of a graph G = (V, E) is a subgraph of G that connects all vertices by exactly one path. In a spanning tree, the parent vertex of a vertex v is denoted as λ(v).4 We call a vertex indexing that fulfills the relation ind(v) > ind(λ(v)) ∀ v ∈ V a T-compatible indexing. 2.3.1 Link Graph. Before the frames are introduced, a few notation requirements have to be fulfilled. The first one is the link enumeration. If the CAD model contains n different links, all these links are enumerated with a simple index i ∈ {1, . . . , n}, thus the set of all links can be denoted by L = {L1 , . . . , Ln }. The second requirement is the usage of joint tuple-indices: Whenever two links Li and L j are connected via a joint, then this joint is denoted as Ji j , where the tuple index (i, j) is element of {1, . . . , n} × {1, . . . , n}.5 Thus, the set of all joints can be denoted by J = {Jij | Li and L j adjacent }. The notation is chosen so that the definition of 4 5
– according to [6, Sec. 4.1.1, 4.1.4] – To work with a unique notation, all joints should be defined so that the index tuple (i, j) is lexicographically sorted (thus i < j).
the link graph GL = (VL , EL ) is easily achieved: The vertex set VL is created by the set of links and the edge set EL is created by the set of joints, VL = L and EL = J . To enable an automated FK computation, a spanning tree of the link graph has to be defined. The spanning tree we denote as TL = (VL , E0L ). For simplicity, we assume that the link enumeration has been conducted in TL -compatible manner (see above). Let L j be an arbitrary link of the spanning tree, then we call Li = λ(L j ) the predecessor link of L j . The link that does not have a predecessor (it is L1 ) is the root of the tree. Configuration Vector. Let D(i j) denote the degree of freedom of joint J(i j) . The set QJ contains all single degrees of freedom q(i, j)k of all joints, like n o QJ = q(i,j)k | J(i j) ∈ J, k ∈ {1, 2 . . . , D(i j) } . (8) The configuration vector q = (q1 , q2 , . . . , qN ) is obtained from the set QJ by first sorting it lexicographically and then casting it into an ordered vector. In case of the triple indices used, the lexicographic comparator 1 composed from the previous D(i j) and the q(t)-dependent joint displacement (t)
(t)
: D(ij) j . The latter is the similar displacement (Eq. 7) to J(i j) j with respect to P(t) (i j) j (t)
D(ij) j : F(t)(ij) 7→ F(t) (ij) j j
p
(t)
(t)
D(i j) j = (D(t)(ij) j )−1 ◦ J(i j) j ◦ D(t)(ij) j .
(12)
The current joint displacement D(t) is composed from the constant joint offset (i j) and current displacement, like (t) D(ij) : F(t) 7→ F(t) (ij)i (i j) j p
D(t) = D (i j) ◦ D(i j) j . (i j)
(13)
Due to the two-frame placing convention, the z-axes of the two frames always coincide. In case of simple joints, the time-invariant D(i j) as well as the timevariant J(ij) are screw displacements. At this point, all entities (links, joints, configuration vector, frames, displacements) that are necessary to compute the forward kinematics have been introduced in a unified manner. This adresses the ambiguities of notation which where pointed out in [14].
4
Computation of Forward Kinematics
In the given context, a forward kinematics (FK) computation means the determination of the poses of all frames F ∈ F , where the pose of each frame F(t) is a (t) (t) function of the mechanism M and the configuration vector q(t) = (q(t) 1 , q2 , . . . , qN ), (t) (t) such that F = f ( M, q ) ∀ F ∈ F . The ‘trick’ to enable CAD-2-SIM to compute the FK automatically for any tree-like mechanism is to define frames in the CAD program according to the two-frame convention that is presented in the previous section. These frames provide a description of the mechanism at timestep t = 1 in its zero reference configuration. F(1) = f ( M, q(1) = 0 ) ∀ F ∈ F . Complexity. While computation of forward kinematics for chain- and tree-like mechanisms is polynomial, the computation for kinematic loops is as complex as the inverse kinematics problem of the chains that build the loops. Within these complexity classes, the runtime of kinematic algorithms is influenced by the algebraic structures that represent the displacements. Notation conventions
like Denavit-Hartenberg or Sheth-Uicker’s two-frame convention deliver a set of parameters that only specify the displacements, but do not fix the algebraic structure, so that the question of runtime is not considered in this paper. Examples. In the following, we describe the principal computations that are implemented in CAD-2-SIM. Due to space limitations, this paper does not contain computations of an example mechanism. However, several examples are available together with the source code of the software. Requirements. For FK computation, two inputs are required by CAD-2-SIM: First, the mechanism needs to be aligned to an arbitrarily chosen, suitable zero pose in the CAD software and needs to be equipped with the set of frames F as described in Sec. 3.3. Second, a Python class according to Fig. 1 has to be implemented that provides the following information: First, for all joints in J the joint type is saved. Second, the spanning tree is provided here. Third, for each frame Fb in the mechanism the dependency is saved: Either the pose of the frame can be determined via the pose of the parent frame λ(b) in the spanning tree, or the frame is part of a parallel substructure. In the latter case, the Python class must also provide dedicated solver routines for those substructures. The computation for trees and the computation for parallel substructures are described in the following two subsections. Initial and Regular Traversals. For both cases – tree-like and parallel structure computation – we distinguish between two kinds of traversals of the mechanism: the initialization phase is identified with timestep t = 1 and described in Par. 4.1.1 and 4.2. Here, all frame poses of the mechanism are computed for the zero reference configuration q(1) = 0. The actual computation of poses for a configuration vector q(t) , 0 is executed in a regular traversal of the algorithm. This is identified with a timestep t > 1 and described in Par. 4.1.2 and 4.2.1. 4.1
Automated FK-Computation for Frames in the Spanning Tree
4.1.1 Initial Traversal. The mechanism is traversed once completely in the initial iteration at timestep t = 1. In this traversal, the initial link displacements D(ijk) (Eq. 10) and the initial joints displacements D(i j) (Eq. 11) are computed. They are stored for each frame-tuple of the spanning tree. In addition, CAD-2-SIM performs validations of preconditions in the initial traversal: it is verified whether the naming and the placing of the frames are correct, according to Sec. 3.3. 4.1.2 Regular Traversals. The mechanism is traversed once completely in each regular iteration at some timestep t > 1. Regular Link Displacements. The displacement of the frame at the end of the link D((t)jk) j is computed as the composition of the current q-dependent D(t) of the (i j) j parent frame F(ij) j and the time-invariant link displacement D(i jk) : D(t) = D(t) ◦ D(i jk) . (i j) j ( jk) j
(14)
Regular Joint Displacements. The displacement of the frame at the end of the joint (t) D(ij) is computed in two steps. First, the current relative joint displacement D(t) (i j) j is computed as in Eq. 13. The second step is analogous to the computation in
Fig. 8: An example mechanism with a parallel substructure. Shown are the zero pose at timestep t = 1 and a second pose at another timestep t > 1, together with (t)
(t) −1 the related displacements, so that the chain D(t) = D(t) ◦ Cb ◦ D(t) a ◦ D(a,b) ◦ (Db ) b b expressed in Eq. 19 is visible.
Eq. 14 for the regular link displacement. To determine D(t) , the joint displacement (i j) j (t) D(ij) is composed with the displacement D(t) of the parent frame: (i j)i D(t) = D(t) ◦ D(t) . (i j) j (i j)i (i j)
(15)
The computation defined by the above equation traverses the entire tree, from the root of the tree (base link) to the leaves of the tree (end-effectors). 4.2
Embedding FK-Computations for Parallel Substructures
Let Fa be the predecessor in the spanning tree of the parallel substructure that can be computed automatically (since it only has one predecessor) and Fb the ‘last’ frame of the parallel substructure9 . We note that Fa and Fb can be arbitrarily chosen ‘on’ the predecessor and the successor links, so typically they are chosen suitable to the substructure geometry10 . The embedding of the parallel substructure into the overall FK computation works in an analogous manner as the embedding of regular joint displacements (Sec. 4.1.2). The displacement of a parallel substructure is therefore a relaxation of a constraint joint displacement to any rigid body transformation. In this interpretation, the parallel substructure simply acts as a complex joint. Since the CAD model defines a feasible (zero) configuration for timestep t = 1, an initial solution for the FK of the parallel substructure is available. This solution can help to select a unique configuration from the ‘symmetric’ solution set for the parallel problem. Initial Parallel Traversal. The computation of initial, time-invariant displacement D (a,b) works in accordance with Eq. 11, so that −1 = (P(1) ◦ (P(1) ). D (a,b) = D(1) a ) b (a,b)
(16)
4.2.1 Regular Parallel Traversals. In the first step (analogous to Eq. 13), the current displacement D(t) of a parallel mechanism is computed via its initial (a,b) (t)
displacement D(a,b) and via the temporal change Db of its last element Fb : (t)
D(t) = D (a,b) ◦ Db . (a,b) 9 10
That again may act as a unique predecessor for another frame. See example in Fig. 8 where the frames Fa and Fb are placed on the ‘base lines’.
(17)
In the second step (analogous to Eq. 15), the relative displacement is combined with the current pose D(t) a of the predecessor Fa to compute the current displacement D(t) of the last element Fb b (t) D(t) = D(t) a ◦ D(a,b) . b
(18)
This functions maps F(0) to F(t) in the passive interpretation. The functions in the b equations above define the following chain: Dd
D (a,b)
(t) (D b )−1
p
p
p
(t)
(t)
(t)
Cb
Db
p
p
(t) (t) (t) (t) F(0) 7−→ F(t) a 7−→ F b 7−→ O b 7−→ Ob 7−→ Fb .
(19)
Fig. 8 provides an example for the situation: the pose of Fb at timestep t is determined by displacement D(t) . b 4.3
Remarks
The displacements that occur in the computation of FK fall into two classes: link displacements can be arbitrary but are time-invariant, whereas joint displacements are time-variant but come in the form of screw displacements in the case of simple joints. The latter constraint is relaxed for complex joints and for parallel substructures. The algorithm in CAD-2-SIM combines these two classes of displacements in the FK computation: a sequential computation ‘along the tree’ is performed for link displacements, whereas a computation relative to the zero configuration is conducted for joint displacements and parallel substructures.
5
Generation of the Mechanism Specification
The specification of a mechanism for Openrave in XML format11 mainly consists of the following information. In one part of the file, all rigid bodies are listed together with their initial pose, their mass properties and their graphical representation. In another part of the file, all joints are listed together with the link connections, their joint type, and their initial pose in terms of axis and anchor point. To generate this information, CAD-2-SIM has to perform only one traversal of the entire mechanism. The main steps can be described as: checking the consistency of namings of the given input files (kinematic frames, dynamic properties, and shape files), checking the consistency of the kinematic frames (namings and placings), and optionally unifying physical units. Finally, some file work is executed and the XML file is generated by Python’s dedicated libraries.
6
Conclusion and Outlook
This paper announces a novel open source software CAD-2-SIM for transferring models of mechanisms from CAD to simulation software and describes how it computes forward kinematics based on the Sheth-Uicker two-frame convention. To derive a uniform description, an indexing based on graph theory is introduced. Since the convention provides a rigorous split between constant and variable displacements, it is demonstrated that the convention can also easily integrate parallel substructures into the kinematics computation. The presented computation can be regarded as a blend of classical computation “along the chain” for 11
see http://openrave.programmingvision.com/wiki/index.php/Format:XML
link displacements and computation via the displacement against Gupta’s zero reference pose for joint displacements. In future, CAD-2-SIM will be extended to support an automated code generation for fast languages like C++. Further, an automated treatment of standard parallel structures can be provided. As a next step, we will document the connections between the Sheth-Uicker’s two-frame convention we have used and the minimal Denavit-Hartenberg convention. Acknowledgement. The research presented in this article was funded by the German Ministry of Education and Research (BMBF) within the projects VI-Bot (Grant Number 01-IW-07003) and CAPIO (Grant Number 01-IW-10001).
References 1. Jorge Angeles. Fundamentals of Robotic Mechanical Systems: Theory, Methods, and Algorithms. Springer-Verlag New York, Inc., Secaucus, NJ, USA, 3rd edition, 2007. 2. Joseph K. Davidson, Kenneth H. Hunt, and Gordon R. Pennock. Robots and Screw Theory: Applications of Kinematics and Statics to Robotics, volume 126. ASME, 2004. 3. J. Denavit and R. S. Hartenberg. A kinematic notation for lower-pair mechanisms based on matrices. Trans. of the ASME. Journal of Applied Mechanics, 22:215–221, 1955. 4. Rosen Diankov. Automated Construction of Robotic Manipulation Programs. PhD thesis, Carnegie Mellon University, Robotics Institute, 2010. 5. Reinhard Diestel. Graph Theory. Springer, 2005. 6. Roy Featherstone. Rigid Body Dynamics Algorithms. Springer, 2008. 7. J. Funda and R. P. Paul. A computational analysis of screw transformations in robotics. IEEE Transactions on Robotics and Automation, 6:348–356, 1990. 8. Krishna C. Gupta. Kinematic analysis of manipulators using the zero reference position description. Int. J. Rob. Res., 5:5–13, 1986. 9. R. S. Hartenberg and J. Denavit. Kinematic Synthesis of Linkages. 1964. 10. Hao-Jan Lai, Edward J. Haug, Sung-Soo Kim, and Dae-Sung Bae. A decoupled flexible-relative co-ordinate recursive approach for flexible multibody dynamics. International Journal for Numerical Methods in Engineering, 32(8):1669–1689, 1991. 11. John McPhee. On the use of linear graph theory in multibody system dynamics. Nonlinear Dynamics, 9(1):73–90, 1996. 12. Richard M. Murray, S. Shankar Sastry, and Li Zexiang. A Mathematical Introduction to Robotic Manipulation. CRC Press, Boca Raton, FL, USA, 1994. 13. Alba Perez and J. M. McCarthy. Dual quaternion synthesis of constrained robotic systems. Journal of mechanical design, 126:425–435, 2004. 14. Bernard Roth. Overview on advanced robotics: Manipulation. In Proceedings ICAR, pages 569–580, 1985. 15. S. Sahu, B.B. Biswal, and Bidyadhar Subudhi. A novel method for representing robot kinematics using quaternion theory. In IEEE Sponsored Conference on Computational Intelligence, Control And Computer Vision In Robotics & Automation, 2008. 16. Lorenzo Sciavicco and Bruno Siciliano. Modelling and Control of Robot Manipulators. Kluwer Academic Publishers, 2nd edition, 1999. 17. Jon M. Selig. Geometric Fundamentals of Robotics, 2nd ed. Springer, Berlin, 2005. 18. P. N. Sheth and John J. Uicker. A generalized symbolic notation for mechanisms. Journal of Engineering for Industry, Series B, 93(70):102–112, 1971. 19. Ulrike Thomas, I. Maciuszek, and Friedrich M. Wahl. A Unified Notation for Serial, Parallel, and Hybrid Kinematic Structures. In ICRA, pages 2868–2873, 2002.
