Arm Coordinate Frames
 Recall from Lecture 3 that we defined a world coordinate frame (w in the figure below) that is fixed to the environment, with standard basis
and origin
. The
axis of world frame points up from the ground, and we normally draw the
axis pointing right and the
axis pointing up.
 We also defined a robot frame (r in the figure below) that moves with the robot. It has basis
and origin
at the point on the ground midway between the two wheel contact points. (The basis and origin vectors of robot frame are expressed in world frame coordinates.)
(i.e. the
axis of robot frame also points up from the ground),
always points directly forward on the robot, and
always points to the robot’s left.
is the CCW positive angle from
to
(i.e. from the world frame
axis direction to the robot frame
axis direction).
 We now define several coordinate frames on the arm (see figure below):
 Frame 0 is attached to link 0 on the arm, the initial link that is rotated by the arm shoulder joint. The
axis of frame 0 points down link 0; the
axis points to robot left, and the
axis points “up” on link 0. The origin of frame 0 is the point where the arm shoulder rotation axis intersects the
plane of robot frame, which is at the fixed location
in robot frame.
 Frame 1 is attached to link 1 on the arm, the second link that is rotated by the arm elbow joint. The
axis of frame 1 points down link 1; the
axis points to robot left, and the
axis points “up” on link 1. The origin of frame 1 is the point where the arm elbow rotation axis intersects the
plane of robot frame, which is at the fixed location
in frame 0.
 Frame 2 is attached to link 2 on the arm, the third link that is rotated by the arm wrist joint. This is also the link that carries the gripper. The
axis of frame 2 points down link 2; the
axis points to robot left, and the
axis points “up” on link 2. The origin of frame 2 is the point where the arm wrist rotation axis intersects the
plane of robot frame, which is at the fixed location
in frame 1.
 Frame g (gripper frame) is also attached to link 2; it models the point where an object could be gripped. It is cooriented with frame 2, and its origin is the center of the circular notch between the gripper fingers, which is at the fixed location
in frame 2.
 The arm joint angles are
: the CCW positive angle from the direction of
to
: the CCW positive angle from the direction of
to
: the CCW positive angle from the direction of
to
.
 Positive values of each rotate the arm down (i.e. these are righthandrule rotations about the corresponding
axes).
 The ranges of motion are
,
,
,
.
 The figure shows the arm in home pose which has
,
,
, and the gripper closed.
 We also define a calibration pose where the arm is pointing straight forward, i.e.
and the gripper is closed.
Forward Kinematics
 In Lecture 3 we also developed the forward position kinematics of the robot mobility, including a mathematical function with inputs
(the robot pose) and output
, a
homogenous transformation matrix taking coordinates in robot frame to world frame.
 We can restate this function using two components:

is a pure translation transform

is a pure rotation about the
axis
 Then
.
 Remember, transforms compose from right to left. Given a 3D point
in robot frame, transform it to world frame
by appending an extra 1 and then leftmultiply by
:
 We now extend this function to include the arm; the inputs will be
, and the output will be
, a
homogenous transformation matrix taking gripper frame to world frame.
 Defining
as a pure rotation about the
axis, continue the composition to include transforms all the way from gripper frame to world frame:
 Using abbreviations
and
, and observing that the product of a pure translation and a pure rotation (in that order) is given simply by copying the upper left
submatrix of the rotation and the upper right
submatrix of the translation,
,
,
 This shows that the transform taking gripper frame to robot frame is the product of a pure translation and a pure rotation:
 In other words,
 the orientation of the gripper in the robot frame
plane is
, i.e. the CCW positive angle from
to
is the sum of the joint angles
 the gripper location in robot frame (the grip point) is
 in this case, both of these results could have been found directly using only basic trigonometry
 but the transform composition formalism with homogenous transformation matrices works the same way for a very broad class of kinematic chains, not just this particular gripper, and it is efficient
Analytic Inverse Kinematics
 Just as inverse position kinematics was very useful for robot mobility, it will also be useful for the arm.
 Conceptually, we would like to literally invert the forward kinematic function.
 Recall that for robot mobility, this meant finding some drive trajectory that would bring the robot to a desired pose
.
 For the arm, we can define the inverse kinematic problem as finding a set of joint angles
that put the gripper at a desired location
and orientation
in the robot frame
plane.
 Later, we’ll show a simple extension that also uses turninplace wheel motions to allow the arm to reach a 3D volume in world frame.
 We’ll also start with one additional simplification: we will only solve a version of the problem where
. That is, we will solve a restricted version of the arm IK problem where the gripper is always held parallel to the ground. This simplifies the math somewhat while still demonstrating the essence of IK. Also, it’s not too much of a compromise in many practical grasping tasks. Below we will also show an extension to allow arbitrary
.
 Inverse kinematics is a wellstudied area. There are a number of approaches; two common ones are analytic (or closed form) IK and numeric (or iterative optimization) IK. We’ll first demonstrate an analytic IK solution for our arm; later we’ll resolve the problem using the numeric approach.
 The basic idea of analytic IK is to try to algebraically invert the forward kinematics equations. When this is possible (and it is not always possible), the approach has several key properites:
 Once the IK equations are found (on paper), they can be coded up, and at runtime their solution(s) can be found (by definition) in constant time. So analytic IK is usually fast.
 Note carefully: there can be more than one solution. For example, in our version of the problem, the elbow could “kink” up or down.
 Unreachable targets are also identified in constant time.
 Here’s one way to solve our version of the IK problem with an analytic approach:
 First, since
, we can immediately derive
(1)
 The same fact implies
and
, which lets us simplify
to
 Now expand the trig identities for angle sums
(2)
(3)
 and add the two identities
(4)
(5)
 Tecnically (25) are a system of 4 equations in 4 unknowns (
), so we should be able to solve for all unknowns.
 This is a little tricky unless we apply some geometric insight. Define the reach triangle with vertices given by the origins of frames 0, 1, and 2 (i.e. the intersections of the shoulder, elbow, and wrist rotation axes with the vertical
plane in robot frame).
 TBD diagram
 Two of the triangle side lengths are
and
; the angle between those two sides is
. Defining the reach vector in world frame as
,
,
the third side has length
(6)
 Apply the cosine angle difference identity and the law of cosines to solve for
:
(7)
(8)
 If
then (8) implies
, but otherwise the given target is unreachable.
 Assuming the target is reachable, use (5) to find
, taking the positive branch of the square root to make the arm “kink up”, i.e. to find a solution with
:
(9)
 Together, (89) determine
. (10)
 The target is unreachable if
, otherwise continue.
 The final job is to calculate
. It will be the difference of the orientation
(11)
of the reach vector and the angle
in the reach triangle between the sides with lengths
and
:
(12)
 TBD diagram

can be found by another application of the law of cosines:
. (13)
 The target is unreachable if
or
.
 Finally, calculate
by (1); the target is reachable iff
.
 Summing up:
is calculated by (10), which depends on (6,8,9); then
is calculated by (12) which depends on (6,11,13); and finally
is given by (1).
 This approach can be extended to include a “yaw” rotation by using the wheels to turn in place, which allows the arm to reach a 3D volume in world frame.
 Let the 3D target location for the gripper be
in world frame, and assume that the robot reference point is at the world frame origin
 if the robot is not at the origin in world frame then subtract the robot location
in the world from
to get the relative vector from the robot reference point to the gripper target location in world frame
(14)
 Calculate the yaw angle
. (15)
 Transform
to
with the robot at orientation
:
 This boils down to
,
or
(16)
where
is the distance from robot frame origin to the projection of the grip point on the ground plane, and
are the coordinates of the grip point in robot frame.
 TBD diagram
 Continue as above with the target
in the robot frame
plane.
 Summing up:
 (first apply (14) to make the grip target relative to the robot location)
 apply (15) to get the robot orientation
from
 apply (16) to get the robot frame grip point
from
 calculate
from (10),
from (12), and
from (1)
 To extend to arbitrary
, not just the special case
as developed so far TBD
 alter the calculation of the reach vector:
 and use
instead of (1)
 TBD diagram

is the absolute CCWpositive tilt angle of the gripper in the robot frame
plane
Types of Kinematic Systems
 In general, a kinematic system is composed of links and joints.
 Links are rigid bodies like our robot’s body and arm segments.
 Each joint connects two links and allows some kind of relative motion.
 We will discuss different types of joints below.
 We have now studied two examples of kinematic systems: the mobility system and the arm on our robot.
 Our robot’s mobility is a planar kinematic system.
 The arm can either be considered another planar kinematic system, if only the wrist, elbow, and shoulder joints are included, or a spatial kinematic system if the robot’s turninplace capability is considered as a fourth “waist” joint.
 For planar kinematic systems link frames are generally described by three degrees of freedom (DoF).
 That is, the minimum number of scalars needed to completely describe the pose of a link relative to an external world frame is three.
 For example, our robot’s mobility system controls the pose of the robot base with respect to the world which we parameterize as
.
 For spatial kinematic systems link frames are generally described by six DoF.
 Using both drive and arm motions, the gripper on our robot’s arm can reach any 3D point in a certain range of heights above the ground, giving 3 translational DoF.
 Two additional rotational DoF can also be independently controlled:
 the pitch of the gripper, i.e. the angle
the plane of the gripper makes with the ground plane.
 the yaw of the gripper, i.e. the compass direction in which it is pointing (on our robot this will always be the same as the robot heading
)
 Due to the particular construction of our robot the gripper roll angle cannot be controlled. This would be the rotation angle about the link 2 frame
axis. That would be the third rotational (and sixth total) DoF.
 In general, the links and joints of a kinematic system form a topological graph, sometimes called the kinematic graph.
 There are various ways to form such a graph, but in one common approach the links are graph vertices and the joints are graph edges.
 Each link has a coordinate frame that moves rigidly with it.
 Each joint has a coordinate transform that relates the pose of the two attached link coordinate frames.
 Kinematic graphs are usually connected, i.e. there is a path in the graph from each vertex to every other vertex.
 Because kinematics is the study of the shape of motion without considering the causes of the motions (physics), kinematics alone cannot say much about the relative motion of disconnected systems.
 Kinematic graph structure is one important way to differentiate classes of kinematic systems.
 The graph of an open chain kinematic system has no loops, i.e. it is a tree.
 Typically one link is identified as the base of the kinematic system, and is the root of the tree.
 Tthe leaves of the tree are considered the effectors.
 Since the graph is a tree, each joint
has an unambiguous parent link
closer to the base and a child link
further from the base.
 The transform of a joint
can be considered to take coordinates from its child link frame to its parent link frame.
 The pose of any link in an open chain system relative to the base
 The arm on our robot is an example of one of the simplest (but also most common) types of open chain kinematic systems: an unbranched serial chain.
 The base of this chain is either the body of the robot, when the joints of the chain are considered to be only the arm wrist, elbow, and shoulder; or if the robot’s turninplace capability is considered as a fourth arm joint (the “waist”), the base of the chain is the ground under the robot.
 The effector of our arm chain is link 2, the link which carries the gripper.
 If the kinematic graph has one or more loops, the system is closed chain.
 Unlike open chain systems, the kinematic graph alone does not unambiguously identify any particular base or effector links.
 Whereas for open chain systems it is generally possible to set each joint independently (ignoring selfcollisions and collisions with obstacles in the world), for closed chain systems some combinations of joint poses are inadmissable.
 The forward kinematics of closed chain systems are typically more complex than for open chain systems because of these constraints on the joint angles.
 If the connectivity of the kinematic graph changes online the system is structure changing.
 For example if a robot with two arms can rigidly attach its hands together, it can form a structure changing kinematic system. When the hands are not attached the system is openchain; when they are attached it is closedchain.
 The terminology serial/parallel is related to the closed/open chain distinction.
 However these terms typically refer specifically to systems with one identified base and one effector.
 Serial systems, like our robot’s arm, connect the base to the effector through intervening links but with no loops in the kinematic graph.
 Typically up to 3 joints are included for planar serial chains and up to 6 for spatial serial chains, corresponding to the number of relative DoF of the effector to the base in each case.
 As we will see below, joints typically allow 1 DoF of relative motion each.
 Though in most cases individual joint DoF do not map directly to the DoF of the effector’s pose — this is why inverse kinematics for serial chains can be nontrivial.
 Parallel systems connect the base and effector with multiple kinematic chains, forming a closedchain graph.
Joint Types
 Joints in a planar kinematic system can theoretically allow up to 3DoF motion between their attached links, and joints in spatial systems could allow up to 6DoF of motion.
 However, in practice, most joints allow only one DoF.
 actuated joints have some type of actuator (like an electric motor) and typically these can only control one DoF at a time.
 unactuated joints are also possible, but not that common in practice except in parallel robots.
 It is usually simpler to build accurate oneDoF joints and then chain them together to achieve multiple DoF.
 A revolute joint has one rotational DoF.
 A prismatic joint has one translational DoF.
 These are the two most important joint types. However, there are certainly more.
 For example, a helical joint (like a screw) has one DoF but allows both rotation
and translation
; the two are coupled by the joint pitch
:
.
 One approach to categorize joint types splits those which maintain a surface of contact, called the lower pairs, from other types, called the higher pairs.
 This terminology originated in the steam age and was developed by people including Franz Reuleaux.
 The lower pairs are practical joints to build in machinery because high stresses can develop if there is only a point or line of contact vs. an entire continuous area.
 For planar systems the only lower pairs are revolute, prismatic, and planar.
 The possible lower pairs in a spatial system include those three plus helical, spherical, and cylindrical joints.
 TBD diagram.
 It has been proven (TBD ref) that these are the only possible lower pairs.
 Planar and sperhical joints have 3 DoF. Cylindrical joints have 2 DoF. When such multiDoF joints are implemented in practice they are typically unactuated, e.g. a ballandsocket assembly is an unactuated spherical joint (with a limited range of motion due to the constraints of attaching to the ball and socket).
Numeric Inverse Kinematics
 Our approach to develop the forward kinematics for our robot arm generalizes fairly easily to the entire class of openchain systems. The pose of any effector
relative to the base
can be calculated as
where
is the childtoparent transform of the parent joint of link
,
is the transform of the next joint on the way to the base, and so on to the last transform
of the last joint in the chain which attaches to the base.
 TBD diagram
 Note that this works without modification even for branched systems with multiple end effectors.
 This calculation can defined as the forward kinematic function: the inputs are the joint angles, and the output of the function is the relative pose of the effector to the base:
 For planar systems the joint transform function for a revolute joint can be defined as
where
is an arbitrary (but fixed by the way the joint is built and assembled to the adjacent link) childtomobility transform, and similarly
is a mobilitytoparent transform (these can be called positioning transforms).
 For planar systems the joint transform function for a prismatic joint can be defined as
 The positioning transforms allow the rotation or translation to effectively work in any direction even though the center mobility transform in the first cse is constructed to only allow rotation about the
axis, and in the second case only
translation.
 These can easly be exteded to the spatial case using
transforms.
 Define the following three functions to extract the
axis rotation and
 and
axis translations from a
homogenous planar transformation matrix:
 For the spatial case it not hard to extend
and
to 3D and to define
, but it is more complex to handle the angle. Spatial rotations have 3 DoF; one option is to generalize
to three functions giving the pitch, roll, and yaw of the effector relative to the base. These are sometimes called Euler angles. However, other representations including quaternions and the exponential map Are often used. The full story is interesting but more than we can cover here.
 Then the forward kinematic function can be considered to have
inputs and 3 outputs:
 The inverse kinematic function would have
outputs and 3 inputs:
 In general
is nonlinear so it can be tricky or impossible to invert analytically, as we did above for the relatively simple arm of our robot.
 An alternative is to numerically approximate the action of
using a local linearization, essentially a firstorder Taylor approximation:
or in matrix and vector notation
(17)
where
and
is the
Jacobian matrix of
at
.
 The top row of the Jacobian is a
vector that locally approximates
as the multiplication of the
vector
by the
vector
 The remaining rows are similar approximations for
and
 Another way to understand the Jacobian is by column: each column
is a
vector of the sensitivities of the EE pose DoF to small movements of joint
relative to its starting value
 Define the original EE pose given joint angles
as
, and the change in EE pose from
to a new location at joint angles
as
.
 Then (17) implies
.
 Subtract
from both sides and multiply by the inverse of the Jacobian, giving
(18)
 As long as the Jacobian is in fact invertible (more on this below), this is effectively an incremental solution to the inverse kinematics problem:
 Let
be the initial joint angles,
, and
be the goal for the EE pose.
 While
where
is a small tolerance value:
 Calculate an updated Jacobain
 Let
for a small stepsize
 Calculate
 Update the joint angles:
 Update the EE pose:

 The Jacobian can only be invertible if it is square, i.e., if the kinematic system has the same number of joints as EE DoF. However, even a square matrix may be noninvertible. Such singular Jacobians arise at specific configurations of the robot, often “outstretched” cases where a robot arm is reaching directly outwards like the calibration pose of our robot. In such configurations small motions in more than one joint yeild infinitesimally equivalent EE motions, and consequently some subspace of EE motions that are normally possible are not reachable. TBD figure.
 Both problems can be worked around, to some extent, by using a pseudoinverse instead of a true inverse. Even better variations on this approach are described in Buss 2009.
 Additional issues that often need to be considered in practice include correct handling of joint motion limits and prioritization of multiple simultaneous goals. See the Buss paper and Baerlocher and Boulic 2004 for useful techniques to address these.