04/10/08 15-494 Cognitive Robotics 1Kinematics15-494 Cognitive RoboticsDavid S. Touretzky &Ethan Tira-ThompsonCarnegie MellonSpring 200804/10/08 15-494 Cognitive Robotics 2OutlineKinematics is the study of how things move.●Homogeneous coordinates●Kinematic chains–Description of the AIBO as a collection of chains●Reference frames●Kinematics and PostureEngine classes●Forward kinematics: calculating limb positions from joint angles. (Straightforward math.)●Inverse kinematics: calculating joint angles to achieve desired limb positions. (Hard.)04/10/08 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]04/10/08 15-494 Cognitive Robotics 4NEWMAT●Tekkotsu uses the NEWMAT package for vector and matrix arithmetic.NEWMAT::ColumnVector v(4), w(4);v = Kinematics::pack(5.75, 30.0, 115);w << 17 << -4.2f << 100 << 1;NEWMAT::Matrix T(4,4);T = v * w.t();Matrix transpose04/10/08 15-494 Cognitive Robotics 5Transformation 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]04/10/08 15-494 Cognitive Robotics 6Transformations 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04/10/08 15-494 Cognitive Robotics 7Kinematic 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J204/10/08 15-494 Cognitive Robotics 8AIBO Kinematic Chains●The AIBO has 9 kinematic chains:–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).04/10/08 15-494 Cognitive Robotics 9Reference Frames●Every joint and every link has an associated reference frame.●Denavit-Hartenberg conventions: all joints 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 404/10/08 15-494 Cognitive Robotics 10Joint vs. Link Reference Frames●The joint reference frame does not rotate with the joint. The link reference frame does.●The x1, y1, z1 joint axes remain fixed with respect to the base frame when the head tilts up or down.●The x2, z2 joint axes rotate with the tilt angle (but not the pan angle.)04/10/08 15-494 Cognitive Robotics 11Leg Reference Frames04/10/08 15-494 Cognitive Robotics 12Leg Reference Frames04/10/08 15-494 Cognitive Robotics 13Reference 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●Note: the distinction between joint and link reference frames is made in the function name, not the reference frame name:–jointToBase(HeadOffset+TiltOffset)–linkToBase(HeadOffset+TiltOffset)04/10/08 15-494 Cognitive Robotics 14Kinematics Class●Tekkotsu uses the ROBOOP package for kinematics calculations.●The Kinematics class provides access to ROBOOP 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.04/10/08 15-494 Cognitive Robotics 15Converting Between Reference Frames●Most common conversion is between the base frame (body coordinates) and a limb frame. baseToJoint(), baseToLink(), jointToBase(), linkToBase() NEWMAT::ReturnMatrix jointToBase(unsigned int joint)●General conversion functions:jointToJoint(), jointToLink(), linkToJoint(), linkToLink() NEWMAT::ReturnMatrix jointToJoint(unsigned int ij,unsigned int oj)04/10/08 15-494 Cognitive Robotics 16Reference Frame Conversion 1●Transform Base to Base:cout.precision(3);cout << setw(7) << kine->jointToBase(BaseFrameOffset);cout << 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.00004/10/08 15-494 Cognitive Robotics 17Reference Frame Conversion 2●Translate base frame to head tilt frame:const float headtilt = state->outputs[HeadOffset+TiltOffset];cout << "Head tilt is " << headtilt * 360/(2*M_PI) << " degrees." << endl;NEWMAT::Matrix TtiltJ(kine->jointToBase(HeadOffset+TiltOffset));NEWMAT::Matrix TtiltL(kine->linkToBase (HeadOffset+TiltOffset));cout << "tilt jointToBase= " << endl << setw(7) << TtiltJ << endl;cout << "tilt linkToBase= " << endl << setw(7) << TtiltL << endl;04/10/08 15-494 Cognitive Robotics 18At ~Zero Degree Tilt AngleHead tilt is 1.25 degrees.tilt jointToBase= 1.000 0.000 0.000 67.500 0.000 0.000 -1.000 0.000 0.000 1.000 0.000 19.500 0.000 0.000 0.000 1.000tilt linkToBase= 1.000 -0.022 0.000 67.500 0.000 0.000 -1.000 0.000 0.022 1.000 0.000 19.500 0.000 0.000 0.000 1.00004/10/08 15-494 Cognitive Robotics 19At ~ -30 Degree Tilt AngleHead tilt is -29.5 degrees.tilt jointToBase= 1.000 0.000 0.000 67.500 0.000 0.000 -1.000 0.000 0.000 1.000 0.000 19.500 0.000 0.000 0.000 1.000tilt linkToBase= 0.871 0.492 0.000 67.500 0.000 0.000 -1.000 0.000 -0.492 0.871 0.000 19.500 0.000 0.000 0.000 1.000cos(-30o) = 0.866sin(-30o) = 0.500●The tilt joint reference frame doesn't rotate with tilt.●The tilt link reference frame does rotate.04/10/08 15-494 Cognitive Robotics 20Interest Points●Interest points on the head, legs,