03/17/10 15-494 Cognitive Robotics 1Kinematics15-494 Cognitive RoboticsDavid S. Touretzky &Ethan Tira-ThompsonCarnegie MellonSpring 201003/17/10 15-494 Cognitive Robotics 2OutlineKinematics is the study of how things move.●Homogeneous coordinates●Kinematic chains–Robots are described as collections of kinematic chains●Reference frames●Kinematics and PostureEngine classes●Forward kinematics: calculating limb positions from joint angles. (Straightforward matrix multiply.)●Inverse kinematics: calculating joint angles to achieve desired limb positions. (Hard.)03/17/10 15-494 Cognitive Robotics 3Homogeneous Coordinates●Represent a point in N-space by an (N+1)-dimensional vector. Extra component is an inverse scale factor.–In “normal” form, last component is 1.–Points at infinite distance: last component is 0.●Allows us to perform a variety of transformations using matrix multiplication: Rotation, Translation, Scaling●Tekkotsu uses 3D coordinates (so 4-dimensional vectors) for everything.v =[xyz1]03/17/10 15-494 Cognitive Robotics 4Transformation Matrices●Let  be rotation angle in the x-y plane.Let dx, dy, dz be translation amounts.Let 1/s be a scale factor.T=[cos sin 0dx−sin  cos 0dy0 0 1dz0 0 0s]Tv =[xcos ysin dx−xsin ycos dyzdzs]=[xcosysin dx/s−xsinycosdy/szdz/s1]03/17/10 15-494 Cognitive Robotics 5Transformations Are Composable●To rotate about point p, translate p to the origin, rotate, then translate back.Translatep =[1 0 0p.x0 1 0p.y0 0 1p.z0 0 0 1]Rotate =[cos sin  0 0−sin  cos 0 00 0 1 00 0 0 1]RotateAboutp, =Translatep ⋅Rotate ⋅Translate−p03/17/10 15-494 Cognitive Robotics 6Kinematic Chains●Sequence of joints separated by links.●We can use transformation matrices to calculate the position of the tip of the chain (joint J2) from the joint angles 0, 1 and the link lengths L1, L2.●Each joint has a rotation transform; each link has a translation transform.L1L2J0J1J203/17/10 15-494 Cognitive Robotics 7AIBO Kinematic Chains●The AIBO has 9 kinematic chains instead of 6 because branched chains were formerly not supported:–4 for the legs–1 for the head (ending in the camera), 1 for the mouth–3 for the IR range sensors●All chains begin at the center of the body (base frame).03/17/10 15-494 Cognitive Robotics 8Chiara Kinematic Chains●The Chiara has 8 major kinematic chains:–Head / camera / IR–Arm–Left front leg–Right front leg (4-dof)–Left middle leg–Right middle leg–Left back leg–Right back leg●Chains are defined in project/ms/config/chiara.kin03/17/10 15-494 Cognitive Robotics 9Reference Frames●Every link has an associated reference frame.●Denavit-Hartenberg conventions: all links move about their reference frame's z-axis.●The head chain:–Base frame 0 z0 = “up”–Tilt joint 1 y1 = “up”–Pan joint 2–Nod joint 3–Camera 403/17/10 15-494 Cognitive Robotics 10Leg Reference Frames03/17/10 15-494 Cognitive Robotics 11Leg Reference Frames03/17/10 15-494 Cognitive Robotics 12Reference Frame Naming Conventions●Use a similar offset-based indexing scheme as for joint names in motion commands and world state vectors:–BaseFrameOffset–HeadOffset + TiltOffset–CameraFrameOffset–LFrLegOffset + ElevatorOffset●Denavit-Hartenberg conventions specify how to express the relationship between one reference frame and the next: d, , r, .–See DH video.03/17/10 15-494 Cognitive Robotics 13Kinematics Class●Tekkotsu contains its own kinematics engine for kinematics calculations, modeled after ROBOOP.●The Kinematics class provides access to basic functionality for forward kinematics.●Global variable kine holds a special Kinematics instance:–Joint values reference WorldState.●PostureEngine is a child of Kinematicsso it can do kinematics calculationstoo. It adds inverse kinematics.–Joint angle results are stored in the PostureEngine instance.03/17/10 15-494 Cognitive Robotics 14fmat●Tekkotsu uses the fmat package to represent coordinates and transformation matrices.●fmat is optimized for efficient representation of small, fixed-size matrices and vectors.fmat::Column<4> v, w;v = fmat::pack(5.75, 30.0, 115, 1);w = fmat::pack(17, -4.2f, 100, 1);fmat::Matrix<4,4> T;T = v * w.transpose();03/17/10 15-494 Cognitive Robotics 15fmat::Transform●Transformation matrices using homogenous coordinates are 4 4.●But the last row is always [0 0 0 1].●So fmat eliminates the last row and overloads the arithmetic operators to make the math work correctly.●fmat::Transform is really a Matrix<3,4>03/17/10 15-494 Cognitive Robotics 16Converting Between Reference Frames●Most common conversion is between the base frame (body coordinates) and a limb frame, or vice versa.●Conversion requires computing a transformation matrix: fmat::Transform linkToBase(unsigned int link) {...} fmat::Transform baseToLink(unsigned int link) {...} fmat::Transform linkTolink(unsigned int ilink, unsigned int olink) {}03/17/10 15-494 Cognitive Robotics 17Reference Frame Conversion 1●Transform Base to Base:fmat::Transform T = kine->linkToBase(BaseFrameOffset);cout << T.fmt(“%8.3f”) << endl;●Result:1.000 0.000 0.000 0.0000.000 1.000 0.000 0.0000.000 0.000 1.000 0.0000.000 0.000 0.000 1.00003/17/10 15-494 Cognitive Robotics 18Reference Frame Conversion 2Translate AIBO head tilt frame to base frame:const float headtilt = state->outputs[HeadOffset+TiltOffset];cout << "Head tilt is " << headtilt * 180/M_PI << " degrees." << endl;fmat::Transform TtiltL(kine->linkToBase (HeadOffset+TiltOffset));cout << "tilt linkToBase=\n" << TtiltL.fmt(“%8.3g”) << endl;03/17/10 15-494 Cognitive Robotics 19At ~Zero Degree Tilt AngleHead tilt is 1.25 degrees.tilt linkToBase= 1.000 -0.022 0.000 67.500 0.000 0.000 -1.000 0.000 0.022 1.000 0.000 19.50003/17/10 15-494 Cognitive Robotics 20At ~ -30 Degree Tilt AngleHead tilt is -29.5 degrees.tilt linkToBase= 0.871 0.492 0.000 67.500 0.000 0.000 -1.000 0.000 -0.492 0.871 0.000 19.500cos(-30o) = 0.866sin(-30o) = 0.50003/17/10 15-494 Cognitive Robotics 21Interest Points●Interest points on the head, legs, and body can be predefined for use in kinematics calculations.●Not yet supported in new kinematics engine.03/17/10 15-494 Cognitive Robotics 22Leg