330 likes | 530 Vues
Forward Kinematics and Configurations. Kris Hauser I400/B659 : Intelligent Robotics Spring 2014. q 2. q 1. Articulated Robot. Robot: usually a rigid articulated structure Geometric CAD models, relative to reference frames
E N D
Forward Kinematics and Configurations Kris Hauser I400/B659: Intelligent Robotics Spring2014
q2 q1 Articulated Robot • Robot: usually a rigid articulated structure • Geometric CAD models, relative to reference frames • A configuration specifies the placement of those frames (forward kinematics)
Forward Kinematics • Given: • A kinematic reference frame of the robot • Joint angles q1,…,qn • Find rigid frames T1,…,Tn relative to T0 • A frame T=(R,t) consists of a rotation R and a translation t so that T·x = R·x + t • Make notation easy: use homogeneous coordinates • Transformation composition goes from right to left:T1·T2 indicates the transformation T2 first, then T1
T2ref T3ref T1ref T4ref Kinematic Model of Articulated Robots: Reference Frame L2 T0 L3 L1 L0
T1(q1) q1 Rotating the first joint T1(q1) =T1ref·R(q1) T0 T1ref L0
Where is the second joint? T2(q1) ? T2ref T0 q1
Where is the second joint? T2ref T0 q1 T2parent(q1) = T1(q1) ·(T1ref)-1·T2ref
After rotating joint 2 q2 T2R T0 q1 T2(q1,q2) = T1(q1)·(T1ref)-1·T2ref·R(q2)
After rotating joint 2 q2 T2R T0 q1 Denote T2->1ref= (T1ref)-1·T2ref(frame relative to parent) T2(q1,q2) = T1(q1) ·T2->1ref·R(q2)
T2(q1,q2) T3(q1,..,q3) T1(q1) T4(q1,…,q4) General Formula Denote (ref frame relative to parent) L2 T0 L3 L1 L0
Generalization to tree structures • Topological sort: p[k] = parent of link k • Denote (frame i relative to parent) • Let A(i) be the list of ancestors of i (sorted from root to i)
To 3D… • Much the same, except joint axis must be defined (relative to parent) • Angle-axis parameterization
Generalizations • Prismatic joints • Ball joints • Cylindrical joints • Spirals • Free-floating bases From LaValle, Planning Algorithms
qn q=(q1,…,qn) q3 q1 q2 Configuration Space • A robot configuration is a specification of the positions of all robot frames relative to a fixed coordinate system • Usually a configuration is expressed as a “vector” of parameters
3-parameter representation: q = (x,y,q) In a 3-D workspace q would be of the form (x,y,z,a,b,g) Rigid Robot workspace robot reference direction q y reference point x
q2 q1 Articulated Robot q = (q1,q2,…,q10)
q 3-D cylinder embedded in 4-D space q 2p q’ robot y q y x x Configuration Space • Space of all its possible configurations • But the topology of this space is in general not that of a Cartesian space S1 R2S1
C = S1 x S1 Configuration Space • Space of all its possible configurations • But the topology of this space is in general not that of a Cartesian space
C = S1xS1 Configuration Space • Space of all its possible configurations • But the topology of this space is in general not that of a Cartesian space
C = S1xS1 Configuration Space • Space of all its possible configurations • But the topology of this space is in general not that of a Cartesian space
Some Important Topological Spaces • R: real number line • Rn: N-dimensional Cartesian space • S1: boundary of circle in 2D • S2: surface of sphere in 3D • SO(2), SO(3): set of 2D, 3D orientations (special orthogonal group) • SE(2), SE(3): set of rigid 2D, 3D translations and rotations (special Euclidean group) • Cartesian product A x B, power notation An = A x A … x A • Homeomorphism ~ denotes topological equivalence • Continuous mapping with continuous inverse (bijective) • Cube ~ S2 • SO(2) ~ S1 • SE(3) ~ SO(3) x R3
q2 q1 What is its topology? (S1)7xI3 (I: Interval of reals)
q 2 q q q q q t(s) 1 n 0 4 3 Notion of a (Geometric) Path • A path in C is a piece of continuous curve connecting two configurations q and q’:t : s [0,1] t (s) C
Examples • A straight line segment linearly interpolating between a and b • t(s) = (1-s) a + s b • What about interpolating orientations? • A polynomial with coeffients c0,…,cn • t(s) = c0 + c1s + … + cnsn • Piecewise polynomials • Piecewise linear • Splines (B-spline, hermite splines are popular) • Can be an arbitrary curve • Only limited by your imagination and representation capabilities
q 2 q q q q q t(t) 1 n 0 4 3 Notion of Trajectory vs. Path • A trajectory is a path parameterized by time:t : t [0,T] t (t) C
q workspace 2p robot reference direction y q y reference point x x Translating & Rotating Rigid Robot in 2-D Workspace configuration space What is the placement of the robot in the workspace at configuration (0,0,0)?
q workspace 2p robot reference direction y q y reference point x x Translating & Rotating Rigid Robot in 2-D Workspace configuration space What is the placement of the robot in the workspace at configuration (0,0,0)?
q workspace What is this path in the workspace? 2p robot reference direction y q P y reference point x x What would be the path in configuration space corresponding to a full rotation of the robot about point P? Translating & Rotating Rigid Robot in 2-D Workspace configuration space
Klamp’t Python API • world = WorldModel() • world.readFile([some file]) • robot = world.robot(0) [if the world has only one robot] • A robot’s configuration is a list of numbers • robot.getConfig() • robot.setConfig(q) automatically performs forward kinematics • It does not necessarily transform like a vector! • Robot-specific interpolation function: robot.interpolate(a,b,u) • A robot’s frames are given as a list of RobotModelLink’s • link = robot.getLink([index or name]) • (R,t) = link.getTransform() • 3D rigid transform utilities in se3.py