바로 내려받기 - robotics@snutranslate this...

INTRODUCTION TO ROBOTICS

MECHANICS, PLANNING, AND CONTROL

F. C. Park and K. Lynch

March 14, 2012

Contents

1 Preview 1

2 Mobility, Configuration Space, and Task Space 112.1 Mobility and Configuration Space of Rigid Bodies . . . . . . . . 12

2.1.1 Mobility of Rigid Bodies . . . . . . . . . . . . . . . . . . . 122.1.2 Configuration Space of Rigid Bodies . . . . . . . . . . . . 14

2.2 Mobility and Configuration Space of Robots . . . . . . . . . . . . 152.2.1 Mobility of Robot Mechanisms . . . . . . . . . . . . . . . 152.2.2 Grubler’s Formula for Planar Robot Mechanisms . . . . . 182.2.3 Grubler’s Formula for Spatial Robot Mechanisms . . . . . 222.2.4 Robot Configuration Space . . . . . . . . . . . . . . . . . 252.2.5 Differential Representation of Kinematic Constraints . . . 27

2.3 Task Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 302.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 312.5 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

3 Rigid-Body Motions 433.1 Rotations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

3.1.1 Definition and Properties . . . . . . . . . . . . . . . . . . 443.1.2 Euler Angles . . . . . . . . . . . . . . . . . . . . . . . . . 453.1.3 Exponential Coordinates . . . . . . . . . . . . . . . . . . . 503.1.4 Unit Quaternions . . . . . . . . . . . . . . . . . . . . . . . 57

3.2 Rigid-Body Motions . . . . . . . . . . . . . . . . . . . . . . . . . 593.2.1 Definition and Properties . . . . . . . . . . . . . . . . . . 593.2.2 Physical Interpretation . . . . . . . . . . . . . . . . . . . . 613.2.3 Screw Motions . . . . . . . . . . . . . . . . . . . . . . . . 64

3.3 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 703.4 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 733.5 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74

4 Forward Kinematics of Open Chains 854.1 Denavit-Hartenberg Representation . . . . . . . . . . . . . . . . . 85

4.1.1 Assigning Link Frames . . . . . . . . . . . . . . . . . . . . 864.1.2 Why Four Parameters are Sufficient . . . . . . . . . . . . 89

iii

4.1.3 Manipulator Forward Kinematics . . . . . . . . . . . . . . 904.1.4 Examples . . . . . . . . . . . . . . . . . . . . . . . . . . . 91

4.2 Product of Exponentials Formula . . . . . . . . . . . . . . . . . . 944.2.1 Formulation and Properties . . . . . . . . . . . . . . . . . 944.2.2 Examples . . . . . . . . . . . . . . . . . . . . . . . . . . . 964.2.3 Relation with the Denavit-Hartenberg Representation . . 984.2.4 A Second Formulation . . . . . . . . . . . . . . . . . . . . 99

4.3 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1004.4 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102

5 Differential Kinematics of Open Chains 1095.1 Particle Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . 1095.2 Angular Velocities . . . . . . . . . . . . . . . . . . . . . . . . . . 1135.3 Spatial Velocities . . . . . . . . . . . . . . . . . . . . . . . . . . . 1155.4 Manipulator Jacobian . . . . . . . . . . . . . . . . . . . . . . . . 118

5.4.1 Space Jacobian . . . . . . . . . . . . . . . . . . . . . . . . 1185.4.2 Body Jacobian . . . . . . . . . . . . . . . . . . . . . . . . 1235.4.3 Relationship between the Space and Body Jacobian . . . 124

5.5 Statics of Open Chains . . . . . . . . . . . . . . . . . . . . . . . . 1255.5.1 Spatial Forces . . . . . . . . . . . . . . . . . . . . . . . . . 1265.5.2 Static Analysis and the Virtual Work Principle . . . . . . 128

5.6 Singularities . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1305.7 Manipulability . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1355.8 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1375.9 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 1405.10 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142

6 Inverse Kinematics of Open Chains 1596.1 Analytic Inverse Kinematics . . . . . . . . . . . . . . . . . . . . . 161

6.1.1 6R Elbow-Type Arm . . . . . . . . . . . . . . . . . . . . . 1616.1.2 Generalized 6R Elbow-Type Arms . . . . . . . . . . . . . 1646.1.3 Stanford-Type Arms . . . . . . . . . . . . . . . . . . . . . 170

6.2 Numerical Inverse Kinematics . . . . . . . . . . . . . . . . . . . . 1716.3 Inverse Kinematics of Redundant Open Chains . . . . . . . . . . 1746.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1756.5 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 1766.6 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 177

7 Kinematics of Closed Chains 1817.1 Inverse and Forward Kinematics . . . . . . . . . . . . . . . . . . 184

7.1.1 3×RPR Planar Parallel Mechanism . . . . . . . . . . . . . 1847.1.2 Stewart-Gough Platform . . . . . . . . . . . . . . . . . . . 1867.1.3 General Parallel Mechanisms . . . . . . . . . . . . . . . . 187

7.2 Differential Kinematics . . . . . . . . . . . . . . . . . . . . . . . . 1887.2.1 Stewart-Gough Platform . . . . . . . . . . . . . . . . . . . 1887.2.2 General Parallel Mechanisms . . . . . . . . . . . . . . . . 190

iv

7.3 Singularities . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1937.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1967.5 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 198

8 Form and Force Closure 2018.1 Contact Models . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2028.2 Frictionless Grasps . . . . . . . . . . . . . . . . . . . . . . . . . . 205

8.2.1 Form Closure and Static Equilibrium . . . . . . . . . . . . 2058.2.2 A Motivating Planar Example . . . . . . . . . . . . . . . 2058.2.3 A Convex Hull Test for Form Closure . . . . . . . . . . . 2098.2.4 A Computational Test for Form Closure . . . . . . . . . . 210

8.3 Grasps with Friction . . . . . . . . . . . . . . . . . . . . . . . . . 2148.3.1 A Motivating Planar Example . . . . . . . . . . . . . . . 2148.3.2 A Convex Hull Test for Planar Force Closure . . . . . . . 2158.3.3 Nguyen’s Theorem for Planar Force Closure . . . . . . . . 2168.3.4 Spatial Force Closure for Rigid Bodies Subject to Three

Point Contacts with Friction . . . . . . . . . . . . . . . . 2188.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2218.5 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 223

Bibliography 231

Index 235

v

Chapter 1

Preview

Robotics is a relatively young field that has set itself the rather ambitious—somemight say impossible—goal of trying to create machines that behave and thinklike humans. This attempt to create intelligent machines naturally leads us tofirst examine ourselves, to ask, for example, why our bodies are designed the waythey are, how our limbs are coordinated, and how we learn and refine complexmotions. The sense that the fundamental questions in robotics are ultimatelyquestions about ourselves is part of what makes robotics such a fascinating andengaging endeavor.

In contrast to the lofty goals set by robotics researchers, the aims of thistextbook are more modest. Our focus will be on the analysis, planning andcontrol of robotic mechanisms, the most common physical manifestationsof intelligent machines. Basically, a mechanism is constructed by connectingrigid bodies (called links) together with joints, so that relative motion betweenadjacent links becomes possible. Actuation of the joints, typically by electricmotors, then causes the robot to move and apply forces in desired ways. Inpractice the rigid bodies may actually be elastic to some degree, and the jointsmay be affected by factors such as elasticity, backlash, friction, and hysteresis;throughout the book we shall not worry too much about these effects, althoughsome of these issues will be addressed in the chapter on robot control.

The links of a mechanism can be arranged in serial fashion, like the familiarserial-chain manipulator shown in Figure 1.1-(a). Mechanisms can also haveclosed loops, such as the parallel mechanism shown in Figure 1.1-(b). In thecase of a serial chain, all of its joints are actuated by motors, while only asubset of the joints may be actuated in the case of mechanisms with closedloops. Wheeled vehicles, often with one or more arms mounted on its body, alsoconstitute another important class of robotic mechanisms that we consider inthis book.

Let us examine the current technology of robot arms a little more closely.As mentioned earlier, the links are moved by actuators, which are typicallyelectric motors (e.g., DC or AC servo motors, or sometimes stepper motors), orsometimes pneumatic or hydraulic cylinders. Ideally motors for robots should be

1

2 Preview

lightweight, operate at low RPM (e.g., under 100), and be able to generate largetorques. Since most currently available motors run at RPMs in the range of 3000and beyond, speed reduction devices with minimal slippage and backlash areoften required (the use of, e.g., belts, sprockets, spur gears can be problematicdue to these reasons; often specially designed low backlash gears, harmonicdrives, and ball screws are used to reduce speed and transmit power). Brakesmay also be attached to quickly stop the robot or to maintain a stationaryposture.

Robots are usually equipped with sensors to measure position and velocity atthe joints. In the case of revolute joints, optical encoders measure the rotationaldisplacement, while angular velocity is measured by tachometers. In the caseof translational joints, displacement and linear velocities are typically measuredby linear variable differential transformers. Forces at the links or at the tip canbe measured by strain gauge force-torque sensors. Additional sensors may beused depending on the nature of the task, e.g., vision cameras, sonar and laserrange finders to locate and measure the position and orientation of objects.

We now provide a preview of each of the chapters, which taken togetherconstitute what we regard as the essential elements of robot mechanics, plan-ning, and control.

Chapter 2: Mobility, Configuration Space, and Task Space

(a) A serial-type industrial manipulator. (b) A parallel mechanism.

Figure 1.1: Serial chain and closed chain robot structures.

In the case of a serial chain robot mechanism such as the industrial manip-ulator of Figure 1.1-(a), typically all of its joints are independently actuated.This is essentially the idea behind the mobility of a mechanism—it is the totalnumber of degrees of freedom (dof) corresponding to the independently actuated

3

joints. For serial chains possessing only one-dof revolute or prismatic joints, themobility is simply the total number of joints. For closed chains like the parallelmechanism shown in Figure 1.1-(b), there are some important subtleties. Fixinga certain subset of the joints to some arbitrary position may automatically fixthe positions of the remaining joints; in such cases not all of the joints can beindependently actuated. For more complicated closed chains involving multi-ple loops and different joint types, determining the degrees of freedom requirescareful analysis.

An equivalent definition of mobility is the minimum number of independentparameters required to specify the position and orientation of each of the linksconstituting the robot mechanism. Based on this definition we shall obtain aformula—Grubler’s formula—relating the number of links and joints comprisinga mechanism with its mobility.

As we discover through various examples, this formula works most of thetime, but not always. It turns out the surest way to understand the mobilityof a robot is in terms of its configuration space. Both planning and controlalso begin with a characterization of the configuration space. We examine theconfiguration spaces of both serial and closed chains.

In order to perform tasks such as grasping and manipulating objects, robotsare typically equipped with end-effectors, e.g., robot hands and grippers. Thetask space, also called the workspace, refers to the space that is reachable bythe end-effector. Clearly this notion of task space depends on the nature ofthe end-effector, and the particular coordinates of the end-effector that are ofinterest. For example, one may be interested only in the set of points in physicalspace that can be reached by the tip of the robot, or one may be interested inthe reachable space of both positions and orientations. One useful way to thinkof the task space is as the configuration space of the end-effector.

Chapter 3: Rigid-Body Motions

In preparation for our later kinematic analysis of robotic mechanisms, this chap-ter considers the problem of how to mathematically describe the motion of rigidbodies. In order to command a robot to pick up a certain object, for example,we need to be able to specify not only the position and orientation of the object,but also the position and orientation of the end-effector..

A convenient way to describe the motion of rigid bodies is to attach refer-ence frames to each of the rigid bodies (including ground), and to describe theposition and orientation of the frames relative to each other. Toward this end,we shall begin with the more familiar analysis of velocities and accelerations ofparticles (or points). Like traditional treatments of particle kinematics, herewe also make use of moving frames, but we shall not retreat to the comfort ofrote formulas for velocities and accelerations; rather, we develop a systematicmethodology for calculating velocities and accelerations that rests on an almosttrivial procedure for differentiating moving frame angular velocities. For stu-dents familiar with velocity and acceleration analysis of particles using movingframes, this first section can be covered quickly as a refresher or skipped en-

4 Preview

Figure 1.2: Assignment of reference frames.

tirely, although students will most likely find this alternative presentation moreappealing and concise.

In the remaining parts of this chapter, we shall introduce the 3 × 3 matrixdescription of rotations, and its various well-known three-parameter represen-tations, e.g., Euler angles and roll-pitch-yaw angles. We shall also introducethe exponential representation for rotations (which can be identified with theangle-axis representation) in a concise and rigorous way based on results fromlinear differential equations. Laying down this groundwork now allows us to,among other things, proceed smoothly to the exponential description of generalrigid body motions. This exponential description of rigid body motions can bedirectly identified with classical screw theory. In addition to the basic rulesfor the matrix representation and manipulation of rigid body motions, we shallalso cover in detail the linear algebraic constructs of screw theory, including theunified description of linear and angular velocities as six-dimensional spatialvelocities.

Chapter 4: Forward Kinematics of Serial Chains

At a kinematic level, the set of inputs to a robot can be regarded as the positionsfor all of its independently controlled joints, which then determines the positionand orientation of the end-effector (the set of outputs). This is precisely theforward kinematics problem for a robot: given a set of joint values, the goalis to determine the position and orientation of the reference frame attached tothe end-effector. In this chapter we discuss two methods for describing the for-ward kinematics of serial chains: the Denavit-Hartenberg (D-H) representation,and the geometrically motivated product-of-exponentials (PoE) formula that isinspired from screw theory.

5

Chapter 5: Differential Kinematics of Serial Chains

Differential kinematics refers to the relationship between joint velocities and thelinear and angular velocities of the end-effector frame. Central to differentialkinematics is the Jacobian of the forward kinematics. This matrix quantita-tively specifies the linear relationship between joint rates and the linear andangular velocities of the end-effector frame, as a function of the robot’s config-uration.

The Jacobian turns out to be central not only for velocity analysis, but alsofor static force analysis. In static equilibrium settings, for example, one maybe interested in determining what forces and torques need to be exerted at theinput joints in order for the end-effector to apply a certain force or momentin a particular direction (consider, for example, a robot attempting to insert apeg into a hole). In the context of static analysis, just as linear and angularvelocities were merged into a single six-dimensional spatial velocity, here we shallalso merge forces and moments into a single six-dimensional spatial force.

The Jacobian is also central to identifying a robot’s kinematic singularities.For serial chains, kinematic singularities are postures in which the end-effectorframe loses the ability to move or rotate in one or more directions. For example,a fully extended human arm is unable to extend any further. One may furtherask how distant a given posture is from a singularity; this naturally leads to thenotion of manipulability, which can be thought of as the ease with which a robotcan move and apply forces in certain directions. We show how manipulabilitycan be used as a criterion to select optimal postures, and even optimal linkdimensions.

Chapter 6: Inverse Kinematics of Serial Chains

As the name implies, inverse kinematics is a problem that is dual to for-ward kinematics: given a desired position and orientation of the end-effectorframe, one seeks to determine the set of joint positions that achieves this de-sired end-effector configuration. For serial chain robots, the inverse kinematicsis in general more involved than the forward kinematics: for a given set of jointvalues there will usually exist a unique end-effector position and orientation, butfor a particular end-effector position and orientation, there may exist multiplesolutions, or even none at all.

In this chapter we shall first examine a general class of six-dof serial chainstructures whose inverse kinematics admits a closed-form analytic solution. Inthe case where analytic closed-form solutions are not available, we shall deriveiterative numerical algorithms for solving the inverse kinematics of general six-dof serial chains. We shall also examine the differential inverse kinematics ofredundant serial chains (that is, those with more than seven degrees of freedom),and present solutions that are based on the generalized inverse of the forwardkinematics Jacobian.

6 Preview

Figure 1.3: A multi-fingered robot hand grasping an object.

Figure 1.4: A planar object immobilized by a set of frictionless point contacts.

Chapter 7: Kinematics of Closed Chains

While serial chains typically have a unique forward kinematics solutions, closedchains may have multiple forward kinematics solutions, in addition to multi-ple solutions for the inverse kinematics. Due to the presence of both actuatedand passive joints, the kinematic singularity analysis of closed chains presentssubtleties not encountered in serial chains. In this chapter we shall examinethe basic concepts and tools for the kinematic analysis of closed chains. Afterdetailed analyses of mechanisms like the planar five-bar linkage and the sym-metric 6-6 Stewart-Gough Platform, we present a systematic methodology forthe kinematic analysis of general closed chains.

7

Chapter 8: Force Closure

This chapter examines a notion that is in some sense dual to mobility: we con-sider here the problem of how to completely restrain a rigid body using a fixednumber of point contacts. This problem is obviously relevant to the graspingof rigid objects with a multifingered robot hand (Figure 1.3), or fixturing anobject with a set of point contact restraints (Figure 1.4). Everyday experiencealso tells us that friction at the point contacts obviously helps matters. Herewe shall develop the necessary mathematical tools to determine when an objectis completely immobilized (in the sense of being able to withstand arbitraryexternal forces) by a set of point contacts with and without friction.

Chapter 9: Dynamics of Serial Chains

In this chapter we derive the dynamic equations for serial chains using the ear-lier product-of-exponentials formalism introduced for kinematic analysis. Thisparticular dynamics formalism is a significant departure from traditional meth-ods that rely on a separate analysis of the linear and angular components of thedynamic equations. Analogous to the notions of forward and inverse kinemat-ics, the forward dynamics problem involves determining the joint trajectoryfor a given input joint torque profile, whereas the inverse dynamics problemis concerned with determining the appropriate joint torque profile for a desiredinput joint trajectory. The main result of this chapter will be a recursive inverseand forward dynamics algorithm for determining the input joint torques for adesired input joint trajectory.

Chapter 10: Trajectory Generation

What sets a robot apart from an automated machine is that it should be easilyreprogrammable for different tasks. Different tasks require different motions,and it would be unreasonable to expect the user to specify the entire time-historyof each joint for every task; clearly it would be desirable for the computer to“fill in the details” from a small set of task input data.

This chapter is concerned with the automatic generation of joint trajectoriesfrom this set of task input data. Often this input data is given in the form of anordered set of joint values, called control points, together with a correspondingset of control times. Based on this data the trajectory generation algorithmproduces a smooth trajectory for each joint that satisfies various user-suppliedconditions.

We shall study some popular algorithms for trajectory generation that wereoriginally developed for computer-aided curve design applications; of particularinterest are cubic splines and Bezier curves. Versions of these algorithms areoffered for the generation of trajectories in both joint space and end-effectorspace (in the latter case, imagine interpolating through an ordered set of end-effector reference frames).

8 Preview

Chapter 11: Motion Planning

This chapter addresses the problem of finding a collision-free path for a robotthrough a cluttered workspace. The most intuitive approach is to work in con-figuration space, which because of the workspace obstacles will now have forbid-den regions. We begin with the most basic of planning algorithms involving gridsearch. A convenient way to navigate through such cluttered regions is throughthe use of artificial potential fields. Briefly, in these methods the obstaclesgenerate artificial forces that repel the robot should it venture too near. Whenthey work, these methods have the advantage of being able to generate collision-free trajectories in real-time. There are a number of subtleties associated withthese methods, however, e.g., the possibility of getting “stuck” in local equilib-ria without reaching the goal configuration, and we discuss some methods toovercome these difficulties. We shall also cover a basic randomized algorithmfor planning collision-free paths, based on rapidly-exploring random trees(RRT).

Chapter 12: Basics of Robot Control

A robot arm can exhibit a number of different behaviors depending on the taskand its environment. It can act as a source of programmed motions for taskssuch as moving an object from one place to another, or tracing a trajectory formanufacturing applications. It can act as a source of forces, for example whengrinding or polishing a workpiece. In tasks such as writing on a chalkboard, itmust control forces in some directions (the force pressing the chalk against theboard) and motions in others (motion in the plane of the board). In certainapplications, e.g., haptic displays, we may want the robot to act like a spring,damper, or mass, controlling its position, velocity, or acceleration in responseto forces applied to it.

In each of these cases, it is the job of the robot controller to convert thetask specification to forces and torques at the actuators. Control strategiesto achieve the behaviors described above are known as motion (or position)control, force control, hybrid motion-force control, and impedance con-trol. Which of these behaviors is appropriate depends on both the task andthe environment. For example, a force control goal makes sense when the end-effector is in contact with something, but not when it is moving in free space. Wealso have a fundamental constraint imposed by mechanics, irrespective of theenvironment: the robot cannot independently control both motions and forcesin the same direction. If the robot imposes a motion, then the environment willdetermine the force, and vice versa.

Most robots are driven by actuators that apply a force or torque to eachjoint. Hence, to precisely control a robot would require an understanding ofthe relationship between joint forces and torques and the motion of the robot;this is the domain of dynamics. Even for simple robots, however, the dynamicequations are usually very complex, Also, to accurately derive the dynamicsrequires, among other things, precise knowledge of the mass and inertia of each

9

link, which may not be readily available. Even if they were, the dynamic equa-tions would still not reflect physical phenomena like friction, elasticity, backlash,and hysteresis.

Most practical control schemes compensate for these errors by using feed-back. One effective method of industrial robot control is to neglect the robot’sdynamics, and instead model each actuator as a scalar second-order linear sys-tem. As such we shall first introduce basic concepts from linear control, andshow how they can be used to effectively control complex multi-dof robots.

This chapter also introduces some basic robot control techniques that as-sume a dynamic model of the robot is available; such feedforward controltechniques use the dynamic model of the robot and its environment to deter-mine actuator control inputs that achieve the desired task. Because of modelingand other errors, feedforward control is rarely used by itself, but is often usedin conjunction with feedback control. After considering feedback and forwardstrategies for model-based motion control, we then examine force control, hybridmotion-force control, and impedance control.

Chapter 13: Contact Models and Manipulation

The focus of the previous chapters has been mostly on the internal characteriza-tion of the robot—its kinematics and dynamics, as well as methods for motionplanning and control. In this chapter we now explicitly consider physical inter-actions between the robot and its environment. The first order of business is tocharacterize the nature of contacts between the robot and objects, or more gen-erally, contact constraints between rigid bodies. As such it becomes necessaryto consider friction, which we considered earlier in the chapter on force closure,at least in the case for point contacts. We examine the equations of motion forrigid body mechanics with friction, formulate the general problem of manipula-tion plannning and grasping, and examine simplifications and assumptions thatlead to certain basic solutions.

Chapter 14: Wheeled Robots

This chapter addresses the kinematics, motion planning, and control of wheeledrobots that are subject to no-slip rolling constraints. Such constraints are funda-mentally different from the loop closure constraints found in closed chains—theformer are holonomic, the latter nonholonomic—and as such we begin witha discussion of nonholonomic constraints. We then examine the kinematicsof some popular wheeled robots: car-like, differential drive, Dubins, and om-nidirectional robots. The controllability problem of determining whether awheeled robot is able to move from a given initial configuration to an arbitraryfinal configuration is then examined. The chapter concludes with a discussionof motion planning and control algorithms for wheeled robots, including theproblem of characterizing and finding optimal paths, and feedback control ofwheeled robots.

10 Preview

Chapter 2

Mobility, ConfigurationSpace, and Task Space

A typical robot is mechanically constructed from several bodies, or links, thatare connected by various types of joints. The robot moves when appropri-ate joints are driven by actuators (e.g., electric motors) that deliver forces ortorques to the joints, either directly or through transmission mechanisms suchas gear trains or cables. Often an end-effector, such as a hand for grasping andmanipulating objects, is attached to some link of the robot. All of the robotsconsidered in this book will have links that can be modeled as rigid bodies.

Given a particular robot, perhaps the most fundamental question we canask is “where is the robot?” The answer to this question is the robot’s config-uration—a specification of the positions of all points of the robot. Since therobot’s links are rigid and of known shape, only a few numbers are needed torepresent the configuration.1 For example, to represent the configuration of adoor, we need only one number: the angle θ of the door opening. To representthe configuration of a point on a plane, we need two numbers: the (x, y) coor-dinates. To represent the configuration of a coin lying on a flat table, we needthree numbers: two specifying the location on the table of a particular point onthe coin, (x, y), and one specifying the coin’s orientation, θ. (See Figure 2.1.)

The minimum number of continuous real numbers needed to represent theconfiguration is the mobility of the robot. Thus a coin (viewed as a robot)lying on a table has mobility three, or equivalently, three degrees of freedom(dof). If our coin could lie heads up or tails up, the configuration space stillhas only three degrees of freedom, since the fourth variable, representing whichside of the coin is up, can only take values in the discrete set {heads, tails}; itdoes not take a continuous range of real values as required by our definition.

Definition 2.1. The configuration of a robot is a complete specification of thepositions of the location of every point of the robot. The minimum number of

1Compare with trying to represent the configuration of a pillow!

11

12 Mobility, Configuration Space, and Task Space

θy

(x, y)

x

θ

(x, y)y

x

(a) (b) (c)

Figure 2.1: (a) The configuration of a door is described by its angle θ. (b) Theconfiguration of a point in a plane is described by (x, y) coordinates. (c) Theconfiguration of a penny on a table is described by (x, y, θ).

real numbers n needed to represent the configuration is the number of degreesof freedom (dof)of the robot. This number is equivalently called the mobilityof the robot. The n-dimensional space containing all possible configurations ofthe robot is called the configuration space.

In this chapter we shall study the configuration space (that is, the spaceof all possible configurations) and mobility of general robots. Since our robotsare constructed of rigid bodies, we first examine the mobility and configurationspace of rigid bodies. We then examine these concepts for general robots, andconclude with a discussion of a robot’s task space, which can intuitively bethought of as the configuration space of a robot’s end-effector.

2.1 Mobility and Configuration Space of Rigid Bod-ies

2.1.1 Mobility of Rigid Bodies

Continuing with the example of the coin lying on the table, choose three pointsA, B, and C fixed to the coin (Figure 2.2(a)). The positions of these pointsin the plane are written (xA, yA), (xB , yB), and (xC , yC). If these points couldbe placed independently anywhere in the plane, we would have six degrees offreedom—two for each of the three points. However, a rigid body is defined asone where all points on the body maintain constant distances from each other,i.e.,

d(A,B) =√

(xA − xB)2 + (yA − yB)2

d(B,C) =√

(xB − xC)2 + (yB − yC)2

d(A,C) =√

(xA − xC)2 + (yA − yC)2

cannot change no matter how the coin moves. Let’s call these constant distancesdAB , dBC , and dAC .

2.1. Mobility and Configuration Space of Rigid Bodies 13

A

B

C

x y

z

(a) (b) (c)

y

x

BCd

dACdAB

A

B

Cy

x

BCd

dACdAB

A

B

C

y

x

BCd

dAC

dAB

A

B

C

y

x

BCd

dACdAB

A

B

C

Figure 2.2: (a) Choosing three points fixed to the penny. (b) Once the locationof A is chosen, B must lie on a circle of radius dAB centered at A. Once thelocation of B is chosen, C must lie at the intersection of circles centered atA and B. Only one of these two intersections corresponds to the “heads up”configuration. (c) The configuration of a penny in three-dimensional space isgiven by the three coordinates of A, two angles to the point B on the sphere ofradius dAB centered at A, and one angle to the point C on the circle defined bythe intersection of the a sphere centered at A and a sphere centered at B.

To determine the number of degrees of freedom, or mobility, of the coin,we first choose the position of point A in the plane (Figure 2.2(b)). We maychoose it to be anything we want, so we have two degrees of freedom to specify,(xA, yA). Once this is specified, however, the constraint d(A,B) = dAB restrictsthe choice of (xB , yB) to those points on the circle of radius dAB centered at A.Thus the two apparent freedoms (xB and yB) are reduced by one constraint,for a total of one actual freedom in choosing the location of B. This freedom isthe angle from A to B. Let’s call this angle θ.

Finally, once we have chosen the location of B, the two apparent freedomsto place C (xC and yC) are reduced by two constraints (d(A,C) = dAC andd(B,C) = dBC), giving a total of zero freedoms. In other words, once we haveplaced A and B, the location of C is fixed. So in fact the coin has exactly threedegrees of freedom in the plane, which can be specified by (xA, yA, θ). If we wereto choose another point on the coin, D, it would appear to be subject to threeconstraints (d(A,D) = dAD, d(B,D) = dBD, d(C,D) = dCD), but in fact theconstraints are redundant—only two of the three constraints are independentof each other. Therefore, points C, D, and all other points on the coin add nomore freedoms nor constraints on the configuration of the coin.

We have been applying the following general rule for determining the numberof degrees of freedom of a system:

degrees of freedom = (sum of freedoms of the points) −(number of independent constraints). (2.1)

We can use this method to determine the number of freedoms of a rigid bodyin three dimensions as well. For example, assume our coin is no longer confined


to the table (Figure 2.2(c)). The three points A, B, and C are now definedby (xA, yA, zA), (xB , yB , zB), and (xC , yC , zC). Point A can be placed freely(three degrees of freedom). The location of point B is subject to the constraintd(A,B) = dAB ; it must lie on the sphere of radius dAB centered at A. Thuswe have 3 − 1 = 2 freedoms to specify, which can be expressed as the latitudeand longitude on the sphere. Finally, the location of point C, which has beenchosen to be on a line with A and B, must lie at the intersection of spherescentered at A and B of radius dAC and dBC , respectively. This intersection isa circle, and the location of point C is described by the angle to a point on thecircle. Thus point C adds 3 − 2 = 1 freedom. Once the position of point Cis chosen, the coin is fixed in space. Summing, we find that the rigid body inthree-dimensional space has six freedoms, which can be described by the threecoordinates of point A, two angles to point B, and one angle to point C. Wediscuss specific representations for the six freedoms of a rigid body in Chapter 3.

Summarizing, we see that a rigid body moving in a two-dimensional plane,which we henceforth call a planar rigid body, has two linear freedoms and oneangular freedom. A rigid body in three-dimensional space, which we henceforthcall a spatial rigid body, has three linear and three angular freedoms. Youshould be able to extend this reasoning to rigid bodies in four-dimensional spaces(four linear freedoms and six angular freedoms, for a total of ten degrees offreedom) and higher. Since we are concerned with physical space, we stop atthree dimensions.

Note that the mobility of a planar rigid body can be obtained instead by firstconsidering the body as a spatial body with six degrees of freedom of a spatialbody, then subtracting the three independent constraints zA = zB = zC = 0.Since our robots will be constructed of rigid bodies, we express Equation (2.1)as

degrees of freedom = (sum of freedoms of the bodies) −(number of independent constraints). (2.2)

Equation (2.2) leads naturally to the study of the mobility of rigid bodies con-nected by joints.

2.1.2 Configuration Space of Rigid Bodies

The space of all possible configurations of a rigid body constitutes its configura-tion space. Let’s first consider the simplest case of a point in three-dimensionalspace: the configuration of the point can be represented uniquely by three co-ordinates, e.g., (x, y, z). It should be clear that fewer numbers cannot be usedto uniquely represent the point’s location. Hence a point in space also has threedegrees of freedom, just like a coin on the table. The linear z coordinate of thepoint in space is different from the angular θ coordinate of the coin, however.We usually write the configuration space of the point as R3, meaning that eachof the three coordinates can be any real number, while we write the configura-tion space of the coin as R2×S1, where S1 is defined as the angle range [0, 2π).Nonetheless, both configuration spaces have three dimensions.

2.2. Mobility and Configuration Space of Robots 15

Alternatively, the four coordinates {(xA, yA), (xB , yB)} provide another wayof representing the configuration space of the coin. Values for these coordinatescannot be chosen arbitrarily, but must satisfy the equation

dAB =√

(xA − xB)2 + (yA − yB)2

where dAB is a given constant. The set of all coordinate values satisfying thisequality constraint then results in a three-dimensional surface embedded in R4.In terms of these four coordinates, the configuration space can thus be repre-sented as a three-dimensional surface in R6.

This construction can be extended to the rigid bodies in three-dimensionalspace as well. This time picking three points A, B, and C on the rigid body,the coordinates for each point are now defined by

pA = (xA, yA, zA)pB = (xB , yB , zB)pC = (xC , yC , zC).

Denoting the fixed distance between A and B by dAB (dBC and dAC are similarlydefined), the nine coordinates must satisfy the three equality constraints

dAB =√

(xA − xB)2 + (yA − yB)2 + (zA − zB)2

dBC =√

(xB − xC)2 + (yB − yC)2 + (zB − zC)2

dAC =√

(xA − xC)2 + (yA − yC)2 + (zA − zC)2.

In terms of these nine coordinates, then, the configuration space of a rigid bodycan be represented as a surface of dimension 9− 3 = 6 embedded in R9. In thenext chapter we shall see other ways of representing the configuration space ofa rigid body, but whatever the representation, its dimension must be six.

2.2 Mobility and Configuration Space of Robots

2.2.1 Mobility of Robot Mechanisms

For now let us take the simplistic view that a robot consists of a set of rigidbodies connected by joints—such assemblies are also called mechanisms orlinkages. Consider once again the door of Figure 2.1(a), consisting of a singlerigid body connected by a hinge joint to “ground” (the wall). We know that thedoor mechanism has only one degree of freedom, the angle θ. If the door werea free-flying spatial rigid body, we saw in the previous section that it has sixdegrees of freedom. Therefore, applying Equation (2.2), the hinge joint providesfive independent constraints on the motion of the door.

On the other hand, if we take a top view of the scene and treat the door as aplanar body, it has only three degrees of freedom, and the hinge joint eliminatestwo of them. Again, we are left with a mechanism with one degree of freedom.For this reason, when dealing with mechanisms, we typically count the degrees


of freedom allowed by the joint rather than the number of motion constraintsit provides.

Figure 2.3 illustrates the basic joints found in typical robots. Every jointconnects exactly two links; we do not allow joints that simultaneously connectthree or more links. The revolute joint (R), also called a hinge joint, allows forrotational motion about the joint axis. The prismatic joint (P), also called asliding joint, allows for translational (rectilinear) motion in the direction of thejoint axis. Both revolute and prismatic joints are one-dof joints; for the revolutejoint, its degree of freedom is parameterized by the angle of rotation about thejoint axis, while the prismatic joint’s degree of freedom is parameterized bythe translation distance along the joint axis. Because most motors are eitherrotary or linear, the actuated joints of robots also tend to be either revolute orprismatic. The screw joint (H), also called a helical joint, is another one-dofjoint whose motion consists of simultaneous rotation and translation.

The cylindrical joint (C) is a two-dof joint that allows for independenttranslations and rotations about a single fixed joint axis. The universal joint(U), also called a Hooke joint, is constructed by serially connecting a pair ofrevolute joints in such a way that their joint axes are orthogonal. From thisdescription it should be clear that the universal joint has two degrees of freedom.The spherical joint (S), also called a ball-and-socket joint, has three degreesof freedom and functions much like our shoulder joint. Two of the freedoms aresimilar to those of the universal joint, and the third is spin about the axis ofthe link attached to the ball. Joints with two or more degrees of freedom arecommonly found in parallel mechanisms (discussed in more detail in Chapter 7)and typically are not actuated.

Let us now return to robots. Viewing a robot purely as a mechanism or link-age (like the door hinge example), earlier we mentioned that its mobility can bedetermined by counting the degrees of freedom allowed by the joints. To moreprecisely state this alternative definition of mobility, consider a mechanism inwhich one of its links can be identified with ground. Its mobility is the totalnumber of degrees of freedom corresponding to the joints that can be indepen-dently actuated. If these independently actuated joints were all locked in place,the mechanism would then be completely immobilized, effectively becoming arigid structure.

For example, the planar four-bar linkage of Figure 2.4(a) is constructed byconnecting four links into a single planar loop, with a revolute joint attachedbetween adjacent links (note that only three links are shown in the figure; groundacts as the fourth link, connecting the two lower joints). Suppose the angle θ1

is fixed to some value. Then, since all the link lengths are assumed known,the remaining angles θ2, θ3, θ4 can be determined from basic trigonometry(see Exercise 9 at the end of this chapter). In fact, given the value of anyother angle, the values for the remaining three angles can be determined. Wetherefore conclude that the four-bar linkage has mobility one; only one joint canbe independently actuated, with the remaining three joints dependent on thisactuated joint.

Based on the above we are thus led to the following alternative characteri-


Figure 2.3: Typical joints used in robots.

zation of mobility:

Proposition 2.1. For a mechanism one of whose links can be identified withground, its mobility coincides with the total degrees of freedom of the indepen-dently actuated joints.

Using this characterization of mobility, in the subsequent sections we shallderive a formula for determining a robot’s mobility based simply on the numberof links, and also the type and number of joints.


Figure 2.4: (a) A four-bar linkage; (b) elbow-up and elbow-down solutions.

Figure 2.5: Coordinates for planar rigid bodies.

2.2.2 Grubler’s Formula for Planar Robot Mechanisms

Grubler’s formula allows one to calculate the mobility of a robot based on thenumber of links, and the number and type of joints. In this section we shallderive this formula for planar robots, i.e., robots whose links all move in a singlefixed plane. First, consider a single rigid body moving in the plane as shown inFigure 2.5(a). Choose a fixed reference frame, and also attach a reference frameto the body. This body-fixed frame is assumed to move with the body, and willalso be referred to as the moving frame. We know that a minimum of threeparameters are required to completely describe the position and orientation ofthis rigid body; a convenient set is (x1, y1) to describe the position of the originof the moving frame, and φ1 to describe the angle between the x-axes of thefixed and moving frames.

Now consider two planar rigid bodies joined by a revolute joint as shown inFigure 2.5(b). If the bodies were not connected, but free to move independently,then six parameters would be required to describe the location of all the bodies(for example, (xi, yi, φi), i = 1, 2 for each rigid body). However, since the bodies


Figure 2.6: A slider-crank mechanism.

are connected by a revolute joint, and assuming the position and orientation ofone of the bodies is known, the location of the other body can be determinedfrom the relative joint angle θ1. Therefore, only 6 − 2 = 4 parameters—forexample, x1, y1, φ1, and θ1—are needed to completely describe the configurationof the robot.

Similarly, for three bodies serially connected by revolute joints, 9−2−2 = 5parameters are required. For planar mechanisms, each revolute joint has theeffect of reducing by two the total number of independent parameters. It is notdifficult to see that the situation will be the same if a revolute joint is replacedby a prismatic joint, or for that matter any other one-dof joint.

If two bodies are connected by a two-dof joint, then five parameters arerequired: three for the position and orientation of one body, and the two jointparameters to describe the relative position and orientation of the second body.Every two-dof joint thus has the effect of reducing by one the total number ofindependent parameters.

With the above, we now state Grubler’s formula for planar robot mech-anisms:

Proposition 2.2. Consider a planar robot mechanism consisting of N links,where by convention the ground is also regarded as a link. Let J denote thetotal number of joints, and let fi be the degrees of freedom of the i-th joint. Themobility M of the robotic mechanism can then be evaluated from the formula

M = 3(N − 1)−J∑i=1

(3− fi)

= 3(N − 1− J) +J∑i=1

fi (2.3)

As a quick check, for the four-bar linkage, N = 4, J = 3, and each fi = 1,resulting in a mobility of M = 3(4− 1− 4) + 4 = 1.

Example 2.1. Slider-Crank MechanismConsider the slider-crank mechanism of Figure 2.6. This mechanism can be


Figure 2.7: (a) k-link planar serial chain. (b) Five-bar planar linkage. (c)Stephenson six-bar linkage. (d) Watt six-bar linkage.

viewed in two ways: (i) the mechanism consists of three revolute joints and oneprismatic joint (J = 4, and each fi = 1), and four links (N = 4; remember thatground is also regarded as a link), or (ii) the mechanism consists of two revolutejoints (fi = 1) and one RP joint (fi = 2; the RP joint is a concatenation of arevolute and prismatic joint), and three links (N = 3; remember that each jointconnects precisely two bodies). In both cases the mobility of the mechanism isone.

Example 2.2. Some Classical Planar LinkagesLet us now apply Grubler’s formula to several classical planar mechanisms asshown in Figure 2.7. For the k-link planar serial chain in (a), N = k + 1 (klinks plus ground), J = k, and since all the joints are revolute, each fi = 1.Therefore,

M = 3((k + 1)− 1− k) + k = k

as expected. For the five-bar linkage of (b), N = 5 (4 links plus ground), J = 5,and since all joints are revolute, each fi = 1. Therefore,

M = 3(5− 1− 5) + 5 = 2.

For the Stephenson six-bar linkage in (c), we have N = 6, J = 7, and fi = 1 forall i, so that

M = (3(6− 1− 7) + 7 = 1.


Figure 2.8: A planar mechanism with two overlapping joints.

Finally, for the Watt six-bar linkage in (d), we have M = 6, J = 7, and fi = 1for all i, so that like the Stephenson six-bar linkage,

M = (3(6− 1− 7) + 7 = 1.

Example 2.3. A Planar Parallel MechanismConsider the planar parallel mechanism illustrated in Figure 2.8. Again, thereare a number of ways to derive its mobility using Grubler’s formula. If all thejoints are modeled as either revolute or prismatic, then the mechanism consistsof seven links (N = 7), eight revolute joints, and one prismatic joint. Notethat three bodies meet at point A; recalling that a joint connects exactly twolinks, the joint at A is not a single revolute joint, but in fact two revolute jointsoverlapping each other. Substituting into Grubler’s formula,

M = 3(8− 1− 9) + 9(1) = 3.

Alternatively, the revolute-prismatic pair connected to point A can be regardedas a single two-dof joint. In this case the number of links N = 7, with sixrevolute joints and a single two-dof revolute-prismatic pair. Grubler’s formulayields

M = 3(7− 1− 9) + 6(1) + 1(2) = 3.

as before.

Example 2.4. Some Exceptions to Grubler’s FormulaConsider the parallelogram linkage of Figure 2.9(a). Applying Grubler’s for-mula, N = 5, J = 6, and fi= 1 for each link, so that M = 3(5− 1− 6) + 6 = 0,which implies that the mechanism is a rigid structure. However, under the as-sumption that the three parallel links are of the same length and that the twohorizontal rows of joints are collinear as implied by the figure, the mechanismcan in fact move, with mobility one.

A similar situation occurs for the five-bar linkage of Figure 2.9(b). If the farleft and far right joints are fixed, then the five-bar linkage should, given that


Figure 2.9: (a) A parallelogram linkage; (b) The five-bar linkage in a regularand singular configuration.

it has mobility two, become a rigid structure. Note however that if the twomiddle links overlap each other as shown in the figure, then these links are ableto rotate freely about the lower pair of overlapping joints. Of course, the linklengths need to be chosen in such a way that this posture is feasible. Moreover,fixing a different pair of joints will render it a rigid structure as expected.

Grubler’s formula is unable to distinguish cases like those just describedfor the parallelogram mechanism and five-bar linkage. Modified versions ofGrubler’s formula that address these and other pathological scenarios have beenproposed, but we do not consider these here, as they are of questionable practicalvalue. Later we shall see that these phenomena are examples of configurationspace singularities arising in closed chains, discussed in detail in Chapter 7.

2.2.3 Grubler’s Formula for Spatial Robot Mechanisms

Analogous reasoning can be applied to develop Grubler’s formula for generalspatial robots. Having established that a minimum of six independent parame-ters is required to describe the position and orientation of a spatial rigid body,we now proceed as we did in the derivation of the planar version of Grubler’sformula. If two spatial rigid bodies are connected by a one-dof joint, then onlyseven independent parameters are required to specify the configuration of thistwo-link system: six parameters for the position and orientation of one body,and one parameter for the joint displacement. More generally, connecting tworigid bodies in space by a single k-dof joint has the effect of reducing the totalnumber of independent parameters required from 12 to 12 − (6 − k) = 6 + k.Based on this observation, Grubler’s formula for spatial robot mecha-nisms can be stated as follows:

Proposition 2.3. Consider a spatial robot mechanism consisting of N links(including ground), J joints (labelled from from 1 to J), and denote by fi denotesthe degrees of freedom of joint i. The mobility of the mechanism can then be


Figure 2.10: The Stewart-Gough Platform.

evaluated from the formula

M = 6(N − 1)−J∑i=1

(6− fi)

= 6(N − 1− J) +J∑i=1

fi. (2.4)

It is worth reminding the reader that the spatial version of this formulashould not be applied to planar mechanisms, and vice versa. We now considersome well-known examples of spatial robots.

Example 2.5. Stewart-Gough PlatformThe parallel platform of Figure 2.10 is the Stewart-Gough platform; it consistsof two platforms—the lower one stationary, the upper one mobile—connectedby six serial chain structures. Each serial chain structure in turn consists ofa universal-prismatic-spherical (UPS) joint arrangement. The total number oflinks (including the fixed lower platform, which is regarded as the ground link)is N = 14. There are six universal joints (each with two degrees of freedom,fi = 2), six prismatic joints (each with a single degree of freedom, fi = 1),and six spherical joints (each with three degrees of freedom, fi = 3). The totalnumber of joints is 18. Substituting these values into the spatial version ofGrubler’s formula,

M = 6(14− 1− 18) + 6(1) + 6(2) + 6(3) = 6.


Figure 2.11: The 3×UPU platform.

The Stewart-Gough platform has mobility six.In some versions of the Stewart-Gough platform the six universal joints are

replaced by spherical joints. Grubler’s formula then indicates that this mecha-nism has twelve degrees of freedom. This result may seem surprising, but it isin fact correct; replacing each universal joint by a spherical joint introduces anextra degree of mobility in each leg, by allowing torsional rotations about theaxis directed along each leg. Note however that this torsional rotation has noeffect on the motion of the mobile platform.

Example 2.6. 3×UPU Parallel MechanismThe next example, shown in Figure 2.11 is also a platform structure, in whichthe two platforms are connected by three UPU serial chain structures. The totalnumber of links is N = 8. There are six universal joints, and three prismaticjoints, making for a total of J = 9 joints. Substituting these values into Grubler’sformula leads to

M = 6(8− 1− 9) + 3(1) + 6(2) = 3.

The 3×UPU mechanism has mobility three according to Grubler’s formula,but constructed prototypes of this mechanism reveal extra degrees of freedomnot predicted by Grubler’s formula or other conventional methods of mobilityanalysis. We shall cover these and other subtle aspects of the 3× UPU platformin Chapter 7.


Figure 2.12: The configuration space of the 2R planar serial chain.

2.2.4 Robot Configuration Space

Previously we saw that the configuration space of a planar rigid body was three-dimensional. If this planar rigid body is now connected to ground with a revolutejoint, then knowledge of the revolute joint angle is sufficient to know the config-uration of the link. In this case this 1R chain has the interval [0, 2π] of angles asits configuration space, which we also denoted S1. Now consider the planar 2Rserial chain of Figure 2.12. Since the link lengths are assumed known, specifyingvalues for the two revolute joints (q1, q2) then completely specifies the positionand orientation of the two links, and thus the serial chain. The configurationspace C in this case is

C = S1 × S1 = {(θ1, θ2) | θi ∈ [0, 2π], i = 1, 2} .

S1 × S1 is also commonly denoted T 2 and referred to as the torus of dimensiontwo. Note that by cutting the torus along the two solid lines indicated in thefigure, the torus can be “flattened” into the rectangle on the right (called the“flat torus”). The same reasoning can be extended to nR spatial serial chains,consisting of n spatial links (one of them the ground link) connected serially byn revolute joints; its configuration space will be the n-dimensional torus Tn.

Let us now return to the planar four-bar linkage of Figure 2.4. The fact thatthe four links always form a closed loop can be expressed in the form of thefollowing three equations:

L1 cos θ1 + L2 cos(θ1 + θ2) + . . .+ L4 cos(θ1 + . . .+ θ4) = 0L1 sin θ1 + L2 sin(θ1 + θ2) + . . .+ L4 sin(θ1 + . . .+ θ4) = 0

θ1 + θ2 + θ3 + θ4 + π = 0.

These equations are obtained by viewing the four-bar linkage as a serial chainwith four revolute joints, in which (i) the tip of link L4 always coincides withthe origin, and (ii) the orientation of link L4 is always horizontal.

These equations, referred to as the kinematic constraint equations or theloop closure equations, represent a set of three equations in four unknowns.


A simple counting argument suggests that there should exist a one-parameterfamily of solutions. Said another way, if one of the joint variables is assigned aspecific value, then values for the remaining joint variables can be obtained asa solution to the above constraint equations. This solution may of course notbe unique (as in the elbow-up and elbow-down configurations), but the numberof solutions will be finite.

A more geometric way to visualize the three constraint equations is to ob-serve that the solutions form a curve in the four-dimensional space of jointvariables (θ1, . . . , θ4). Since the joints are revolute, zero radians can be identi-fied with 2π and its integer multiples, but the essential fact still remains thatthe set of solutions forms a one-dimensional curve residing in a four-dimensionalambient space. This curve is the configuration space of the four-bar linkage. Thefour-bar linkage having mobility one reflects the fact that its configuration spaceis a curve, or one-dimensional. (The reason a curve is one-dimensional is thatlocally it can be approximated by a line. Similarly, a surface is two-dimensionalbecause locally it can be approximated by a plane.)

One could in fact eliminate θ4 altogether from the constraint equations, andreduce the three equations to two:

L1 cos θ1 + L2 cos(θ1 + θ2) + L3 cos(θ1 + θ2 + θ3)− L4 = 0L1 sin θ1 + L2 sin(θ1 + θ2) + L3 sin(θ1 + θ2 + θ3) = 0.

The solutions would still form a one-dimensional curve, but now reside in thethree-dimensional ambient space parametrized by (θ1, θ2, θ3). The configurationspace is still the same, of course, but one dimension of the ambient space hasbeen removed.

The kinematic constraint equations for the five-bar linkage can also be ex-pressed in a similar fashion:

L1 cos θ1 + L2 cos(θ1 + θ2) + . . .+ L5 cos(θ1 + . . .+ θ5) = 0L1 sin θ1 + L2 sin(θ1 + θ2) + . . .+ L5 sin(θ1 + . . .+ θ5) = 0

θ1 + θ2 + . . .+ θ5 + π = 0.

Here the configuration space, which consists of all solutions to the above threeconstraint equations, forms a two-dimensional surface residing inside a five-dimensional ambient space parametrized by (θ1, . . . , θ5).

As the four-bar linkage example shows, the kinematic constraint equationsfor general robots containing one or more closed loops can always be written inthe form

g(θ) =

g1(θ1, . . . , θn)...

gk(θ1, . . . , θn)

= 0, (2.5)

where θ ∈ Rn denotes the vector of joint variables, and g : Rn → Rk, denotesa set of k independent equations, k typically being smaller than n. Constraintsthat can be written in this functional form are known as holonomic con-straints. Since g(θ) = 0 represents a set of k equations in n variables, in


general the solutions to form a multidimensional surface of dimension n− k re-siding in an ambient space of dimension n. In this way the configuration spaceof a robot can be viewed as a multdimensional space.

2.2.5 Differential Representation of Kinematic Constraints

Holonomic Constraints

In the case of robots that are subject to kinematic constraints, e.g., closedchains, a better understanding of the configuration space can be obtained byexamining its differential version. That is, we explicitly write the joint vector θas a function of time t, θ(t), and differentiate the constraint equation g(θ(t)) = 0with respect to t as follows (of course g(θ) must be differentiable with respectto θ):

d

dtg(θ(t)) =

∂g1∂θ1

(θ)θ1 + . . .+ ∂g1∂θn

(θ)θn...

∂gk∂θ1

(θ)θ1 + . . .+ ∂gk∂θn

(θ)θn

=

∂g1∂θ1

(θ) · · · ∂g1∂θn

(θ)...

. . ....

∂gk∂θ1

(θ) · · · ∂gk∂θn

(θ)

θ1

...θn

=

∂g

∂θ(θ)θ. (2.6)

Here θi denotes the time derivative of θi with respect to time t, ∂g∂θ (θ) ∈ Rk×n,

and θ, θ ∈ Rn.The differential version of the constraint g(θ) = 0 is thus of the form

G(θ)θ = 0, G(θ) =∂g

∂θ. (2.7)

Clearly any admissible velocity θ by definition lies in the nullspace of the k× nmatrix G(θ) (recall from linear algebra that the nullspace of a matrix A ∈Rk×n is the set of vectors x ∈ Rn that satisfy Ax = 0; if A is of maximalrank k, then this nullspace is a vector space of dimension n − k). Anotherway to characterize the mobility of a mechanism is by the dimension of thisspace of admissible velocities. More precisely, the mobility is the dimension ofthe nullspace of G(θ). Although this dimension may vary depending on thechoice of θ, provided that g is differentiable, it will only do so over regions “ofmeasure zero.” Throughout “most” of the configuration space, the dimension ofthe nullspace remains constant. Those points at which the dimension changesin fact can be identified with the configuration space singularities alluded toearlier in our mobility examples for closed chains.

Constraints on θ of the form

A(θ)θ = 0, (2.8)


Figure 2.13: A disk rolling on a plane without slipping.

where A(θ) ∈ Rk×n, are called differential constraints. In the above weshowed that the constraint g(θ) = 0 can be expressed in differential form bysetting A(θ) = G(θ). One could also regard g(θ) as being the “integral” ofG(θ); for this reason constraints of the form g(θ) = 0 are called integrableconstraints.

Summarizing the discussion thus far, the configuration space of closed chainslike the five-bar linkage can be mathematically represented by a holonomic con-straint of the form g(θ) = 0, where g : Rn → Rk is differentiable; such anequation implicitly describes a surface of dimension n − k embedded in Rn.This constraint can also be represented in differential form, i.e., Equation (2.7).

Nonholonomic Constraints

We shall now consider an example that gives rise to a different kind of differentialconstraint. Consider the disk of radius r rolling on the plane as shown inFigure 2.13. A body-fixed frame is attached to the center of the disk, in such away that the axis of rotation is always directed along the x-axis; since this frameis attached to the disk, the y- and z-axes rotate about the x-axis at the samerate as the disk. The disk is assumed to always remain normal to the plane(that is, the x-axis of the disk frame always remains parallel to the plane).

Under these assumptions, a minimum of four parameters are needed to com-pletely describe the configuration of the disk. A convenient set is given by thecontact point coordinates (x, y), the steering angle φ, and the angle of rotationθ (see Figure 2.13). The configuration space of the disk is therefore R2 × T 2,where T 2 is the two-dimensional torus parametrized by the angles φ and θ. Thisconfiguration space is four-dimensional.

Let us now express, in mathematical form, the fact that the disk rolls withoutslipping. The disk must always roll in the direction indicated by (cosφ, sinφ),

2.3. Task Space 29

with forward speed rθ: [xy

]= rθ

[cosφsinφ

]. (2.9)

Collecting the four configuration space coordinates into a single vector q =(x, y, φ, θ) ∈ R2×T 2, the above no-slip rolling constraint can then be expressedin the form [

1 0 0 −r cosφ0 1 0 −r sinφ

]q = 0. (2.10)

This is also a differential constraint of the form A(q)q = 0, A(q) ∈ R2×4.Had this been a holonomic constraint, we would have concluded that the

corresponding configuration space was of dimension 4−2 = 2 (four variables, twoconstraints). However, this is clearly incorrect, as the configuration space is ofdimension four. The reason is that the differential constraint of Equation 2.10 isnot integrable. That is, for the given A(q), there does not exist any differentiableg : R4 → R2 such that ∂g

∂q = A(q). Differential constraints that are not integrableare also known as nonholonomic constraints.

For this particular example a direct calculation should be enough to con-vince the reader that the given A(q) is nonintegrable, or nonholonomic. Fornonholonomic constraints one cannot, as we did before for closed chains, drawconclusions about the dimension of the configuration space based on, e.g., sub-tracting the number of constraints from the number of degrees of freedom. Toidentify nonintegrable differential constraints in the general case requires moreadvanced mathematical methods that we do not introduce here.

If the rolling disk were viewed as a simplified kinematic model of a wheeledvehicle, then one could take φ, the rate of forward rolling, and θ, the steering an-gle rotation rate, as the inputs. From experience we know that by appropriatelychoosing φ and θ, the vehicle can be maneuvered to any arbitrary position (x, y),and any arbitrary orientation for φ and θ. Consider, for example, the parallelparking maneuver; we are effectively moving the car sideways by a sequence offorward-backward zig-zig manuevers. The kinematics of wheeled vehicles arethe subject of Chapter 14, where nonholonomic constraints are addressed inmore detail. The results of this section are summarized in the following.

Definition 2.2. Let q ∈ Rn be a configuration vector and q ∈ Rn be its timederivative. A differential constraint is an equation of the form

A(q)q = 0, A(q) ∈ Rk×n. (2.11)

A differential constraint is said to be holonomic if there exists a differentiablemapping g : Rn → Rk, k ≤ n, such that

A(q) =∂g

∂q(q). (2.12)

Otherwise the differential constraint is said to be nonholonomic.


Figure 2.14: Examples of task spaces for various robots: (a) a planar 2R serialchain; (b) a planar 3R serial chain; (c) a spherical 2R serial chain; (d) a 3Rorienting mechanism.

2.3 Task Space

We introduce one more piece of terminology, via an example involving the planar2R serial chain (see Figure 2.14(a)). Suppose the x-y coordinates of the tip areof interest. The region in the plane that the tip can reach is called the taskspace, or alternatively the workspace, of the chain. The task space is distinctfrom the configuration space, in that any point in task space does not specify thefull configuration of the robot. Rather, the task space depends on the physicalnature of the end-effector, and can sometimes be regarded as the configurationspace for the end-effector.

Two mechanisms with different configuration spaces may also have the sametask space. For example, the planar 2R serial chain with links of equal lengththree, and the planar 3R serial chain with links of equal length two (see Fig-ure 2.14(b)), will have the same task space despite having different configurationspaces.

Two mechanisms with the same configuration space may also have differenttask spaces. In the spherical 2R serial chain of Figure 2.14(c), the tip alwayslies on a sphere. Similarly, in the 3R serial chain of Figure 2.14(d), because

2.4. Summary 31

the three joint axes will always intersect at the tip for all joint values, the tipremains fixed. These task spaces are quite different from the task spaces for theplanar 2R and 3R serial chains.

Specification of the task space depends fundamentally on the nature of thetasks that the robot is expected to perform; this is usually manifested in thechoice of end-effector attached to the robot. For example, if the end-effector fora spatial robot were a hand, then not only the position, but the orientation of theend-effector must also be considered. For general spatial robots one typicallyattaches a reference frame to the end-effector; the robot’s task space is thendeclared to be the set of all possible positions and orientations reachable by theend-effector frame.

There may be cases like the earlier planar chain examples where one is onlyinterested in the Cartesian position of the tip. Alternatively, there may becases where one is only interested in the orientation of the end-effector. Forexample, in the 3R serial chain of Figure 2.14(d), although the origin of theend-effector frame always remains stationary, the orientation of the end-effectorframe depends on the choice of joint angles (we shall study the kinematics ofthis orienting device more closely in the next chapter).

The task spaces of serial chain robots will be examined more closely inChapter 7. In particular, it should be clear that the size, shape, and otherqualitative features of the task space reflect the particular capabilities of a robot;in Chapter 7 we shall develop quantitative methods that attempt to measurethe performance of a robot based on properties of its task and configurationspaces.

2.4 Summary

• A robot is mechanically constructed from links that are connected byvarious types of joints. In most cases the links are modelled as rigidbodies. An end-effector such as a hand or gripper is typically attachedto some link of the robot. Actuators deliver forces and torques to thejoints, thereby causing motion of the robot.

• The most widely used one-dof joints are the revolute joint, which allowsfor rotation about the joint axis, and the prismatic joint, which allowsfor translation in the direction of the joint axis. Some common two-dofjoints include the cylndrical joint, which is constructed by serially con-necting a revolute and prismatic joint, and the universal joint, which isconstructed by orthogonally connecting two revolute joints. The spher-ical joint, also known as ball-in-socket joint, is a three-dof joint thatfunctions like the human wrist joint.

• The mobility of a robot refers to the number of independent parametersrequired to completely specify the position and orientation of each linkconstituting the robot. In the case of a robot one of whose links can be


identified with ground, the mobility is also the total degrees of freedomcorresponding to the independently actuated joints.

• Grubler’s Formula allows one to calculate the mobility of a robot basedon the number of links, and the number and type of joints. For planarrobots, i.e., robots whose links all move in a single fixed plane, the planarversion of Grubler’s Formula is given by

M = 3(N − 1− J) +J∑i=1

fi,

where M is the mobility of the robot, N is the number of links (includ-ing the ground link), J is the number of joints (the joints are labelledfrom 1 to J), and fi is the degrees of freedom of joint i. This formulais derived in part from the observation that in order to specify the po-sition and orientation of a rigid body moving in the plane, a minimumof three parameters—two for the position and one for the orientation—are required. For general spatial robots, i.e., robots whose links move inthree-dimensional space, the spatial version of Grubler’s Formula is givenby

M = 6(N − 1− J) +J∑i=1

fi.

The spatial version of Grubler’s Formula is derived in part from the obser-vation that in order to specify the position and orientation of a rigid bodyin three-dimensional space, a minimum of six parameters—three each forthe position and orientation—is required.

• The configuration of a robot is a complete specification of the position ofevery point on the robot. If all of the robot’s links are rigid bodies, then theconfiguration is a complete specification of the position and orientation ofevery link of the robot. The configuration space is the set of all possibleconfigurations of the robot. The mobility of a robot is just the dimensionof the configuration space, or equivalently, the minimum number of real-valued parameters required to specify the configuration. The configurationspace is regarded as an intrinsic property of a robot, determined entirelyby its physical properties such as link lengths and joint limits. In somecases the presence of, e.g., obstacles in the environment, may prevent therobot from reaching certain configurations. The part of the configurationspace that is reachable by the robot is called the free space.

• In the case of robots that are subject to kinematic constraints of the formg(θ) = 0, representing a set of k equations in n unknowns, with n > k, suchconstraints are known as holonomic constraints. For robots subjectto holonomic constraints, their configuration space can be viewed as amultidimensional space of dimension n − k residing in an ambient spaceof dimension n. Assuming θ varies with time t, the equation g(θ(t)) = 0

2.4. Summary 33

can be differentiated with respect to t to yield the following differentialversion of the holonomic constraint:

G(θ)θ = 0, G(θ) =∂G

∂θ(θ),

where G(θ) is an k × n matrix.

• Some robots, e.g., wheeled robots whose wheels are assumed to roll with-out slipping, are subject to a differential constraint of the form

A(θ)θ = 0,

where A(θ) is an k×n that cannot be expressed as the differential of somefunction g(θ), i.e., there does not exist any g(θ), g : Rn → Rk such that

A(θ) =∂g

∂θ(θ).

Such differential constraints are said to be nonholonomic constraints,or nonintegrable constraints. For robots subject to nonholonomicconstraints, the dimension of the configuration space cannot be obtainedby subtracting the number of constraints from the number of variables;rather, the configuration space will have dimension greater than this num-ber.

• Given a spatial robot with a reference frame attached to its end-effector,the task space is defined to be the set of all positions and orientationsachievable by the end-effector frame. More generally, once the coordinatesof interest for a robot’s end-effector have been specified (e.g., the Cartesianposition, orientation, or both position and orientation), the robot’s taskspace is defined to be the portion of the space defined by the coordinatesthat is reachable by the robot’s end-effector.

Notes and References

In the classical kinematics literature, structures that consist of links connectedby joints are called mechanisms or linkages. The mobility analysis of mech-anisms is treated in most texts on mechanism analysis and design, e.g., [9]. Ingeneral, a robot’s kinematic configuration space has the mathematical structureof a differentiable manifold; some accessible introductions to differential geome-try are [22], [8]. Configuration spaces are further examined in a motion planningcontext in [14], [5].


2.5 Exercises

1. Use the planar version of Grubler’s formula to determine the mobility of themechanisms shown in Figure 2.15.

(a) (b)

(c) (d)

(e) (f)

Figure 2.15: A collection of planar mechanisms.

2. (a) Use an appropriate version of Grubler’s formula to determine the mobilityof the 6×RUS platform of Figure 2.16.(b) What happens to the mobility if the universal joints are replaced by sphericaljoints?

2.5. Exercises 35

Figure 2.16: 6×RUS platform.

3. Use Grubler’s formula to determine the mobility of the six parallel mecha-nisms shown in Figure 2.17.

4. Three identical open chain SRS arms are grasping an object as shown inFigure 2.18.(a) Find the mobility of this system of cooperating robots.(b) Suppose there are now a total of n 7-dof open chain arms grasping theobject. What is the mobility of this system of robots?(c) Suppose the spherical wrist joint in each of the n seven degree of freedomopen chains is replaced by a universal joint. What is the mobility of the overallsystem?

5. Determine the mobility of the chain formed by the human arm and robotarm as shown in Figure 2.19.

6. Consider a spatial parallel mechanism consisting of a moving plate connectedto the fixed plate by n identical open chains. In order for the moving plate tohave mobility six, how many kinematic degrees of freedom should each leg have,as a function of n? For example, if n = 3, then the moving plate and fixedplate are connected by three open chains; how many degrees of freedom shouldeach open chain have in order for the moving plate to move with six degrees offreedom? Solve the above for arbitrary n.

7. Consider the parallel mechanism shown in Figure 2.20, in which six legsof identical length are connected to a fixed and moving platform via sphericaljoints. Determine the mobility of this mechanism using Grubler’s formula, and


(a) (b)

(c) (d)

(e) (f)

Figure 2.17: A collection of spatial parallel mechanisms.

illustrate all possible motion types of the upper platform.

8. The mobile manipulator of Figure 2.21 consists of a 6R arm and multifin-

2.5. Exercises 37

Figure 2.18: Three cooperating open chain arms grasping a common object.

Figure 2.19: Human-robot cooperating device.

gered hand mounted on a mobile base with a single-wheel. The wheel rotateswithout slip, and its axis of rotation is always parallel to the ground.(a) Ignoring the multifingered hand, describe the configuration space of themobile manipulator.(b) Now suppose the robot hand rigidly grasps the refrigerator door handle, andopens the door using only its arm. Assume the wheel is locked in place, so thatthe mobile base does not move. After the door has been opened, what is themobility of the mechanism formed by the arm and open door?(c) A second identical mobile manipulator comes along, and after parking itsmobile base, also rigidly grasps the refrigerator door handle. What is the mo-bility of the mechanism formed by the two arms and the open referigeratordoor?

9. In this exercise we consider the configuration space of the four-bar linkageof Figure 2.22, this time projected onto the space of the two joints attached tothe ground link. Label the joints and link lengths, and attach a fixed reference


Figure 2.20: A 6× SS platform.

Figure 2.21: Mobile manipulator for Exercise 9.

frame as shown in the figure. It follows that

A(θ) = (a cos θ, a sin θ)T

B(ψ) = (g + b cosψ, b sinψ)T

From ‖A(θ)−B(ψ)‖2 = h2 for all values of θ and ψ, we get

b2 + g2 + 2gb cosψ + a2 − 2(a cos θ(g + b cosψ) + ab sin θ sinψ) = h2

Grouping the coefficients of cosψ and sinψ, the above equation can be expressed

2.5. Exercises 39

Figure 2.22: Planar four-bar linkage.

in the formα(θ) cosψ + β(θ) sinψ = γ(θ),

where

α(θ) = 2gb− 2ab cos θβ(θ) = −2ab sin θγ(θ) = h2 − g2 − b2 − a2 + 2ag cos θ.

We wish to obtain ψ as a function of θ. To do so, first divide both sides ofEquation 9 by

√α2 + β2 to obtain

α√α2 + β2

cosψ +β√

α2 + β2sinψ =

γ√α2 +B2

Now, referring to Figure 2.22-(b), the angle φ is given by φ = tan−1(βα ), andEquation 9 becomes

cos(ψ − φ) =γ√

α2 + β2.

Therefore,

ψ = tan−1(β

α)± cos−1

(γ√

α2 + β2

).

(a) Note that a solution exists only if γ2 ≤ α2 + β2. What are the physicalimplications if this constraint is not satisfied?(b) Note that for each value of input angle θ, there exists two possible values ofthe output angle ψ. What do these two solutions look like?(c) Draw the configuration space of the mechanism in θ-ψ space for the followinglink length values: a = b = g = h = 1.(d) Repeat (c) for the following link length values: a = 1, b = 2, h =

√5, g = 2.

(e) Repeat (c) for the following link length values: a = 1, b = 1, h = 1, g =√

3.


Figure 2.23: The 3× PUP platform.

10. Figure 2.23 shows a 3× PUP platform, in which three identical PUP legsconnect a fixed base to a moving platform. The P joints on both the fixedbase and moving platform are arranged symmetrically as shown. Recalling thatthe U joint consists of two revolute joints connected orthogonally, each R jointconnected to the moving platform has its joint axis aligned in the same directionas the platform P joint. The R joint connected to the fixed base has its jointaxis orthogonal to the base P joint.(a) Use an appropriate version of Grubler’s formula to find the mobility.(b) Reasoning intuitively, determine whether the mobility you obtained in part(a) is correct. If not, try to explain the discrepancy without resorting to adetailed kinematic analysis.

11. Figure 2.24 shows a spherical four-bar linkage, in which four links (oneof the links is the ground link) are connected by four revolute joints to form asingle loop closed chain. The four revolute joints are arranged so that they lieon a sphere, and such that their joint axes intersect at a common point.(a) Use appropriate version of Grubler’s formula to find the mobility.(b) Describe the configuration space.(c) Assuming a reference frame is attached to the center link, describe its taskspace.

12. Consider the two-link planar open chain of Figure 2.25, whose tip coordi-nates are given by

x = 2 cos θ1 + cos(θ1 + θ2)y = 2 sin θ1 + sin(θ1 + θ2).

2.5. Exercises 41

Figure 2.24: The spherical four-bar linkage.

Figure 2.25: Two-link planar RR open chain.

(a) What is its configuration space?(b) What is the set of all points the tip can reach?(c) Suppose there is an infinitely long wall at x = 1 and x = −1. What is itsfree space?

13. Consider the slider-crank mechanism of Figure 2.6. A rotational motion atthe revolute joint fixed to ground (the “crank”) causes a translational motionat the prismatic joint (the “slider”). Suppose the two links connected to thecrank and slider are of equal length. Determine the configuration space of this


mechanism, and draw its projected version on the space defined by the crankand slider joint variables.

14. (a) Consider a planar 3R open chain with link lengths (starting from thefixed base joint) 5, 2, and 1, respectively. Considering only the Cartesian pointof the tip, draw its task space.(b) Now consider a planar 3R open chain with link lengths (starting from thefixed base joint) 1, 2, and 5, respectively. Considering only the Cartesian pointof the tip, draw its task space. Which of these two chains has a larger taskspace?(c) A not so clever designer claims that he can make the task space of any planaropen chain larger simply by increasing the length of the last link. Explain thefallacy behind this claim.

15. Determine if the following differential constraints are holonomic or non-holonomic:(a)

(1 + cos q1)q1 + (1 + cos q2)q2 + (cos q1 + cos q2 + 4)q3 = 0.

(b)

−q1 cos q2 + q3 sin(q1 + q2)− q4 cos(q1 + q2) = 0q3 sin q1 − q4 cos q1 = 0.

Chapter 3

Rigid-Body Motions

This chapter examines mathematical representations for the position and orien-tation of spatial rigid bodies. The initial setting will be the same one introducedin the earlier chapter: rigid bodies occupy a three-dimensional physical spaceendowed with a length scale, and a fixed right-handed reference frame (the fixedframe) for physical space has been established. Right-handed reference framesare also attached to each of the rigid bodies (the body frames).

In this setting, we saw earlier that the orientation of the body frame relativeto the fixed frame can be described by a 3× 3 matrix belonging to the SpecialOrthogonal Group SO(3); the combined position and orientation can be furtherdescribed by a 4 × 4 matrix belonging to the Special Euclidean Group SE(3).SO(3) and SE(3) are respectively three- and six-dimensional groups, in thesense that only three of the nine entries of a rotation matix are independent,and only six of the twelve entries in an SE(3) matrix are independent. SO(3)and SE(3) are also referred to as the group of rotations and rigid-bodymotions, respectively.

This chapter begins with a treatment of the most basic family of three-parameter representations for SO(3), the Euler angles. This is followed bya treatment of the exponential coordinates on SO(3), which requires somebasic results from linear differential equations that we also review here. Theunit quaternions, which is a four-parameter representation for rotations—onlythree of the four parameters are independent—closely related to the exponentialcoordinates, is also introduced.

Various six-parameter representations for SE(3) are then considered. Thepreferred choice, as we shall demonstrate repeatedly throughout the remainderof the book, is a six-dimensional exponential coordinates on SE(3) which, itturns out, can also be identified with the six parameters of screw theory.This screw-theoretic representation of rigid-body motions forms the basis formuch of the kinematic and dynamic analysis to follow later.

43

44 Rigid-Body Motions

3.1 Rotations

3.1.1 Definition and Properties

In the previous chapter we saw that the orientation of a spatial rigid body canbe described by a 3 × 3 rotation matrix R, in which the three columns of Rcan be regarded as the unit axes of the reference frame attached to the body.The set of rotation matrices forms the Special Orthogonal Group SO(3), whosedefinition we repeat here:

Definition 3.1. The Special Orthogonal Group SO(3), also known as thegroup of proper rotation matrices, is the set of all 3 × 3 real matrices R thatsatisfy (i) RTR = I, and (ii) detR = 1.

We now list some basic properties of rotation matrices:

Proposition 3.1. The inverse of a rotation matrix R ∈ SO(3) is its transpose,RT .

This follows from the first condition RTR = I and the definition of matrixinverse. Another consequence is that RRT = I. �

Proposition 3.2. The product of two rotation matrices is also a rotation ma-trix.

GivenR1, R2 ∈ SO(3), it readily follows that (R1R2)T (R1R2) = I and detR1R2 =detR1 ·detR2 = 1, verifying that the product of two rotation matrices is indeedanother rotation matrix. �

Proposition 3.3. For any vector x ∈ R3 and R ∈ SO(3), the vector y = Rxis of the same length as x.

The above follows from ‖y‖2 = yT y = xTRTRx = xTx = ‖x‖2. As we shall seelater, y can be regarded as a rotated version of x about some fixed axis passingthrough the origin. �

The final property of rotation matrices offers a physical interpretation ofthe product of two rotation matrices. We first introduce some notation. Giventwo reference frames {a} and {b}, the orientation of frame {b} as seen fromframe {a} will be represented by the rotation matrix Rab. From this definitionit follows readily that Raa = I.

Proposition 3.4. RabRbc = Rac.

To prove this result, introduce a third reference frame {c}, and define the unitaxes of frames {a}, {b}, and {c} by the triplet of orthonormal unit vectors{xa, ya, za}, {xb, yb, zb}, and {xc, yc, zc}, respectively. Suppose that the unitaxes of frame {b} can be expressed in terms of the unit axes of frame {a} by

xb = r11xa + r21ya + r31zayb = r12xa + r22ya + r32zazb = r13xa + r23ya + r33za.

(3.1)

3.1. Rotations 45

Similarly, suppose the unit axes of frame {c} can be expressed in terms of theunit axes of frame {b} by

xc = s11xb + s21yb + s31zbyc = s12xb + s22yb + s32zbzc = s13xb + s23yb + s33zb.

(3.2)

The above equations can also expressed more compactly as

[xb yb zb

]=

[xa ya za

] r11 r12 r13

r21 r22 r23

r31 r32 r33

(3.3)

[xc yc zc

]=

[xb yb zb

] s11 s12 s13

s21 s22 s23

s31 s32 s33

. (3.4)

Note that the 3×3 matrix of Equation (3.3) is Rab, while that of Equation (3.4)is Rbc. Substitution of (3.3) into (3.4) yields

[xc yc zc

]=[xa ya za

] r11 r12 r13

r21 r22 r23

r31 r32 r33

s11 s12 s13

s21 s22 s23

s31 s32 s33

.(3.5)

From the above it follows that RabRbc = Rac. � If moreover we choose {c} ={a}, then another consequence of the above relation is that R−1

ab = Rba.

3.1.2 Euler Angles

Earlier we made the case that a rotation matrix can be parametrized by threeindependent coordinates. Here we introduce one popular parametrization ofSO(3), the ZYX Euler angles. One way to visualize these angles is through thewrist mechanism shown in Figure 3.1. The ZYX Euler angles (α, β, γ) refer tothe angle of rotation about the three joint axes of this mechanism. In the figurethe wrist mechanism is shown in its home position, i.e., when all three jointsare set to their zero positions.

Four reference frames are defined as follows: frame {0} is the fixed (ground)frame, while frames {1}, {2}, and {3} are attached to the various links of thewrist mechanism as shown. When in the zero position, all four reference frameshave the same orientation. We now consider the relative frame orientations R01,R12, and R23. First, it can be seen that R01 depends only on the angle α, andis given by

R01 =

cosα − sinα 0sinα cosα 0

0 0 1

. (3.6)

Similarly, R12 depends only on β, and is given by

R12 =

cosβ 0 sinβ0 1 0

− sinβ 0 cosβ

. (3.7)


γ

β

α

{0}x

y

z

{1}

xy

z

{2}xy

zx y

z {3}

Figure 3.1: Wrist mechanism corresponding to the ZYX Euler angles.

Finally, R23 depends only on γ, and is given by

R23 =

1 0 00 cos γ − sin γ0 sin γ cos γ

. (3.8)

R03 = R01R12R23 is therefore given by

R03 =

cαcβ cαsβsγ − sαcγ cαsβcγ + sαsγsαcβ sαsβsγ + cαcγ sαsβcγ − cαsγ−sβ cβsγ cβcγ

, (3.9)

where sα is shorthand for sinα, cα for cosα, etc.In order for these ZYX Euler angles to be a valid coordinate parametriza-

tion for SO(3), we need to show that for any arbitrary rotation matrix R, thereexists (α, β, γ) such that Equation 3.9 is satisfied. This also amounts to showingthat the wrist mechanism of Figure 3.1 can reach all orientations. We prove thisconstructively as follows. Let rij be the ij-th element of R. Then from Equa-tion 3.9 we know that r2

11 + r221 = cos2 β; as long as cosβ 6= 0, or equivalently

β 6= ±90◦, we have

β = tan−1

(sinβcosβ

)= tan−1

(−r31

±√r211 + r2

21

). (3.10)

Define the function atan2(y, x) to be the arctangent function tan−1 yx that also

takes into account the signs of x and y. For example, atan2(1, 1) = π4 , while

3.1. Rotations 47

γ

β

α

=90o

Figure 3.2: Configuration corresponding to β = 90◦ for ZYX Euler angles.

atan2(−1,−1) = − 3π4 . Then the possible values for β can be expressed as

β = atan2(−r31,√r211 + r2

21) (3.11)

andβ = atan2(−r31,−

√r211 + r2

21) (3.12)

In the first case β will be in the range −90◦ ≤ β ≤ 90◦, while in the second caseβ lies in the range 90◦ ≤ β ≤ 270◦.

Assuming the value for β obtained from above is not ±90◦, α and γ can thenbe determined from the following relations:

α = atan2(r21, r11) (3.13)γ = atan2(r32, r33) (3.14)

In the event that β = ±90◦, there exists a one-parameter family of solutions forα and γ. This is most easily seen from Figure 3.2: if for example β = 90◦, α andγ then represent rotations (in the opposite direction) about the same verticalaxis. Hence, if (α, β, γ) = (α, 90◦, γ) is a solution for a given rotation R, thenany triple (α′, 90◦, γ′) where α′ − γ′ = α− γ, is also a solution.


Algorithm for Computing the ZYX Euler Angles

Objective: Given R ∈ SO(3), find angles α, β ∈ [0, 2π] and β ∈ [−π/2, π/2]that satisfy

R(α, β, γ) =


, (3.15)

where sα is shorthand for sinα, cα for cosα, etc. Denote by rij the ij-th entryof R.

(i) If r31 6= ±1, set

β = atan2(−r31,√r211 + r2

21) (3.16)

α = atan2(r21, r11) (3.17)γ = atan2(r32, r33), (3.18)

where the square root is taken to be positive.

(ii) If r31 = 1, then β = π/2, and a one-parameter family of solutions for αand γ exists. One possible solution is α = 0, and γ = atan2(r12, r22).

(iii) If r31 = −1, then β = −π/2, and a one-parameter family of solutions forα and γ exists. One possible solution is α = 0, and γ = −atan2(r12, r22).

From the earlier wrist mechanism illustration of the ZYX Euler angles itshould be evident that the choice of zero position for β is, in some sense, ar-bitrary. That is, we could just as easily have defined the home position of thewrist mechanism to be as in Figure 3.2; this would then lead to another three-parameter representation (α, β, γ) for SO(3). In fact, what we’ve described isprecisely the definition of the ZYZ Euler angles. The resulting rotation matrixcan be obtained via the following sequence of rotations:

R(α, β, γ) =

cα −sα 0sα cα 00 0 1

cβ 0 sβ0 1 0−sβ 0 cβ

cγ −sγ 0sγ cγ 00 0 1

=

cαcβcγ − sαsγ −cαcβsγ − sαcγ cαsβsαcβcγ + cαsγ −sαcβsγ + cαcγ sαsβ−sβcγ sβsγ cβ

. (3.19)

Just as we showed for the ZYX Euler angles, for every rotation R ∈ SO(3),there exists a triple (α, β, γ) that satisfies R = R(α, β, γ) for R(α, β, γ) as givenin Equation (3.19). (Of course, the resulting formulas will differ from those forthe ZYX Euler angles.)

From the wrist mechanism interpretation of the ZYX and ZYZ Euler angles,it should be evident that for Euler angle parametrizations of SO(3), what reallymatters is that rotation axis 1 is orthogonal to rotation axis 2, and that rotation

3.1. Rotations 49

α

X

Y

Z

X'^

Y'Z'

β

X

Y

Z

X''^

Y''^Z''^

γ

X

Y

Z

X'''^

Y'''^Z'''^

Figure 3.3: Another physical interpretation of the ZYX Euler angles.

axis 2 is orthogonal to rotation axis 3 (axis 1 and axis 3 need not necessarily beorthogonal to each other). Specifically, any sequence of rotations of the form

Rot(Axis1, α) · Rot(Axis2, β) · Rot(Axis3, γ), (3.20)

where Axis1 ⊥ Axis2 and Axis2 ⊥ Axis3, can serve as a valid three-parameterrepresentation for SO(3). In a subsequent section we shall see how to express arotation about an arbitrary axis.

A Second Physical Interpetation of Euler Angles

Interestingly, the mathematical representation of the ZYX Euler angles admitsanother physical interpretation, which we now describe via Figure 3.3. Given aframe in the identity configuration (that is, R = I), we first rotate this frameby an angle γ about the X-axis of the fixed frame, followed by an angle β aboutthe Y -axis of the fixed frame, and finally by an angle α about the Z-axis of thefixed frame.

Let us derive the explicit form of a vector v ∈ R3 (expressed as a columnvector using fixed frame coordinates) that is rotated about the fixed frame X-axis by an angle γ. The rotated vector, denoted v′, will be

v′ =

1 0 00 cos γ − sin γ0 sin γ cos γ

v. (3.21)

If v′ is now rotated about the fixed frame Y -axis by an angle β, then the rotatedvector v′′ can be expressed in fixed frame coordinates as

v′′ =

cosβ 0 sinβ0 1 0

− sinβ 0 cosβ

v′. (3.22)


Finally, rotating v′′ about the fixed frame Z-axis by an angle α yields the vector

v′′′ =

cosα − sinα 0sinα cosα 0

0 0 1

v′′. (3.23)

If we now imagine v to be the unit axes of the reference frame in the identityorientation R = I, then after applying the above sequence of rotations to thereference frame, its final orientation will be

R(α, β, γ) =



1 0 00 cγ −sγ0 sγ cγ

· I=


. (3.24)

This product of three rotations is exactly the same as that for the ZYX Eulerangles. Hence, we see that the same product of rotations admits two differentphysical interpretations: as a sequence of rotations with respect to the movingframe (ZYX Euler angles), or, reversing the order of rotations, as a sequence ofrotations with respect to the fixed frame. Such a dual interpretation will existfor any sequence of rotations of the form (3.20), in which consecutive axes areorthogonal.

In summary, the main features of Euler angle parameterizations are:

• The Euler angles physically represent three successive rotations takenabout the unit axes of either the moving reference frame, or of the fixedreference frame.

• The first axis must be orthogonal to the second axis, and the second axismust be orthogonal to the third axis, but the first and third axes need notnecessarily be orthogonal to each other.

• The angle of rotation for the first and third rotations ranges in value overa 2π interval, while that of the second rotation ranges in value over aninterval of length π.

3.1.3 Exponential Coordinates

We now introduce another three-parameter representation for rotations, theexponential coordinates. While the previous rotation representations essen-tially expressed a rotation matrix as a product of three rotations, each of whichdepended on a single parameter, the exponential coordinates parametrize a ro-tation matrix in terms of a single rotation axis (represented by a vector of unitlength), together with an angle of rotation about that axis. This representationis most naturally introduced in the setting of linear differential equations, whosemain results we now review.

3.1. Rotations 51

Some Basic Results from Linear Differential Equations

To motivate the discussion, consider the scalar linear differential equation

x(t) = ax(t) (3.25)

with x(t) ∈ R, a ∈ R constant and initial condition x(0) = x0 (x(t) denotes thederivative of x(t) with respect to t). Equation (3.25) has solution

x(t) = eatx0. (3.26)

It is also useful to remember the series expansion of the exponential function:

eat = 1 + at+(at)2

2!+

(at)3

3!+ . . . (3.27)

With these scalar preliminaries, consider now the vector linear differentialequation

x(t) = Ax(t) (3.28)

where x(t) ∈ Rn, A ∈ Rn×n is constant, and the initial condition is x(0) = x0.From the scalar version, one can conjecture a solution of the form

x(t) = eAtx0 (3.29)

where the matrix exponential eAt now needs to be defined in a meaningful way.Again mimicking the scalar case, we define the matrix exponential to be

eAt = I +At+(At)2

2!+

(At)3

3!+ . . . (3.30)

The first question to be addressed is under what conditions this series converges,so that the matrix exponential is well-defined. It can be shown that if A isconstant and finite, this series is always guaranteed to converge to a finite limit;the proof can be found in most texts on ordinary linear differential equationsand shall not be repeated here.

The second and more relevant question is whether Equation (3.29) is indeeda solution to (3.28). Taking the time derivative of x(t) = eAtx0,

ddtx(t) =

(ddte

At)x0

=(A+ A2

2! + A3

3! + . . .)x0

= AeAtx0

= Ax(t),

(3.31)

which proves that x(t) = eAtx0 is indeed a solution. That this is a uniquesolution follows from the basic existence and uniqueness result for linear ordinarydifferential equations, which we invoke here without proof.

Note that in the third line of (3.31), A could also have been factored to theright, i.e.,

d

dtx(t) = eAtAx0. (3.32)


More generally, it is always true that

d

dteAt = AeAt = eAtA. (3.33)

While the matrix exponential eAt is defined as an infinite series, closed formexpressions are often available. If A can be expressed as A = PDP−1 for someconstant D ∈ Rn×n and constant invertible P ∈ Rn×n, then

eAt = I +At+ (At)2

2! + . . .

= I + (PDP−1)t+ (PDP−1)(PDP−1) t2

2! + . . .

= P (I +Dt+ (Dt)2

2! + . . .)P−1

= PeDtP−1.

(3.34)

If moreover D is diagonal , i.e., D = diag{d1, d2, . . . , dn}, then its matrix expo-nential is particularly simple to evaluate:

eDt =

ed1t 0 · · · 0

0 ed2t · · · 0...

.... . .

...0 0 · · · ednt

. (3.35)

We summarize the above results in the following proposition:

Proposition 3.5. Let A ∈ Rn×n be constant, and x(t) ∈ Rn. The lineardifferential equation x = Ax, with initial condition x(0) = x0, has solution

x(t) = eAtx0 (3.36)

where

eAt = I + tA+t2

2!A2 +

t3

3!A3 + . . . . (3.37)

The matrix exponential eAt further satisifies the following properties:

(i) ddte

At = AeAt = eAtA.

(ii) If A = PDP−1 for some D ∈ Rn×n and invertible P ∈ Rn×n, theneAt = PeDtP−1.

(iii) Given constant A,B ∈ Rn×n, then eAeB = eC , where

C = A+B +12

[A,B] +112{[A, [A,B]]− [B, [A,B]]}+ . . . , (3.38)

where the notation [A,B] is defined to be [A,B] = AB −BA.

(iv) If AB = BA, then eAeB = eA+B.

(v) (eA)−1 = e−A.

3.1. Rotations 53

Figure 3.4: The vector p(0) is rotated by an angle θ about the axis ω, to p(θ).

The third property is known as the Baker-Campbell-Hausdorff formula; co-efficients for higher order terms can be derived by matching terms of the samedegree in the series expansion of the exponentials. Also, the fifth propertyfollows by setting B = −A in the fourth property.

A final linear algebraic result useful in finding closed form expressions foreAt is the Cayley-Hamilton Theorem, which we state here without proof:

Proposition 3.6. Let A ∈ Rn×n be constant, with characteristic polynomial

p(s) = det(Is−A) = sn + cn−1sn−1 + . . .+ c1s+ c0. (3.39)

Thenp(A) = An + cn−1A

n−1 + . . .+ c1A+ c0I = 0. (3.40)

Exponential Coordinates

In the exponential coordinate representation for rotations, a rotation is repre-sented by a single axis of rotation together with a rotation angle about the axis.Referring to Figure 3.4, suppose a three-dimensional vector p(0) is rotated byan angle θ about the fixed rotation axis ω, to p(θ); here we assume all quantitiesare expressed in fixed frame coordinates, and ‖ω‖ = 1. This rotation can beachieved by imagining that p(0) is subject to a rotation about ω at a constant


rate of 1 rad/sec, from time t = 0 to t = θ. Let p(t) denote this path; thevelocity of p(t), denoted p, is then given for any time t by

p = ω × p. (3.41)

To see why this is true, observe that p(t) traces a circle of radius ‖p(t)‖ sinφabout the ω-axis (φ is the angle between p(t) and ω), and that ‖p(t)‖ = ‖p(0)‖.Then p = ω× p(t) is tangent to the path as a velocity vector should be, and itsmagnitude is ‖ω‖ · ‖p(t)‖ sinφ = ‖p(t)‖ sinφ, corresponding to a rotation of 1rad/sec as required.

We now make use of an alternative way of expressing the cross-productbetween two vectors. To do this we introduce the following notation:

Definition 3.2. Given a vector ω ∈ R3, define

[ω] =

0 −ω3 ω2

ω3 0 −ω1

−ω2 ω1 0

. (3.42)

Note that the matrix [ω] as defined above is skew-symmetric; that is,

[ω] = −[ω]T .

With this representation it is a simple calculation to verify the following prop-erty:

Proposition 3.7. Given two vectors x, y ∈ R3, their cross product x × y canbe obtained as

x× y = [x]y, (3.43)

where [x] is the skew-symmetric matrix representation of x. Also,

[x× y] = [x][y]− [y][x]. (3.44)

Another useful property involving rotations and skew-symmetric matrices isthe following:

Proposition 3.8. Given any ω ∈ R3 and R ∈ SO(3), the following alwaysholds:

R[ω]RT = [Rω]. (3.45)

To prove this result, write R explicitly in terms of its rows and expandR[ω]RT , i.e., letting rTi be the i-th row of R,

R[ω]RT =

rT1 (ω × r1) rT1 (ω × r2) rT1 (ω × r3)rT2 (ω × r1) rT2 (ω × r2) rT2 (ω × r3)rT3 (ω × r1) rT3 (ω × r2) rT3 (ω × r3)

=

0 −rT3 ω rT2 ωrT3 ω 0 −rT1 ω−rT2 ω rT1 ω 0

= [Rω],

(3.46)

3.1. Rotations 55

where the second line makes use of the determinant formula for 3× 3 matrices,i.e., if M is a 3 × 3 matrix with columns {a, b, c}, then detM = aT (b × c) =cT (a× b) = bT (c× a). �

With the above results, the differential equation (3.41) can now be expressedas

p = [ω]p, (3.47)

with initial condition p(0) = 0. This is a linear differential equation of the formx = Ax that we have studied earlier; its solution is given by

p(t) = e[ω]tp(0). (3.48)

Since t and θ are interchangeable (as a result of assuming that p(t) rotates at aconstant angular speed of 1 rad/sec) the above can also be written

p(θ) = e[ω]θp(0). (3.49)

We now derive a closed-form expression for e[ω]θ. Here we make use of theCayley-Hamilton Theorem for [ω]. First, the characteristic polynomial associ-ated with the 3× 3 matrix [ω] is given by

p(s) = det(Is− [ω]) = s3 + s. (3.50)

The Cayley-Hamilton Theorem then implies p([ω]) = 0, or

[ω]3 = −[ω]. (3.51)

Let us now expand the matrix exponential e[ω]θ in series form. Replacing [ω]3

by −[ω], [ω]4 by −[ω]2, [ω]5 by −[ω]3 = [ω], and so on, we obtain

e[ω]θ = I + [ω]θ + [ω]2 θ2

2! + [ω]3 θ3

3! + . . .

= I +(θ − θ3

3! + θ5

5! − · · ·)

[ω] +(θ2

2! −θ4

4! + θ6

6! − · · ·)

[ω]2,(3.52)

which, recalling the series expansions for sin θ and cos θ, simplifies to

e[ω]θ = I + sin θ [ω] + (1− cos θ)[ω]2. (3.53)

Earlier we showed that multiplying a vector v ∈ R3 by the exponential matrixe[ω]θ above amounts to rotating v about the axis ω by an angle θ. Our next taskis to show that for any given rotation matrix R ∈ SO(3), we can always find anω and θ such that R = e[ω]θ. We shall call [ω]θ the logarithm of R. In this waywe can assert that ωθ ∈ R3 provides another three-parameter representation forrotations. Expanding each of the entries for e[ω]θ = I + sin θ [ω] + (1− cos θ)[ω]2

leads to cθ + ω21(1− cθ) ω1ω2(1− cθ)− ω3sθ ω1ω3(1− cθ) + ω2sθ

ω1ω2(1− cθ) + ω3sθ cθ + ω22(1− cθ) ω2ω3(1− cθ)− ω1sθ

ω1ω3(1− cθ)− ω2sθ ω2ω3(1− cθ) + ω1sθ cos θ + ω23(1− cθ)

,(3.54)


where ω = (ω1, ω2, ω3), and we use the shorthand notation sθ = sin θ, cθ = cos θ.Setting this equal to R ∈ SO(3) leads to the following:

r32 − r23 = 2ω1 sin θr13 − r31 = 2ω2 sin θr21 − r12 = 2ω3 sin θ.

(3.55)

Therefore as long as sin θ 6= 0, or θ 6= kπ for k = 0,±1,±2, . . ., we can write

ω1 = 12 sin θ (r32 − r23)

ω2 = 12 sin θ (r13 − r31)

ω3 = 12 sin θ (r21 − r12).

(3.56)

The above equations can also be expressed in skew-symmetric matrix form, i.e.,

[ω] =

0 −ω3 ω2

ω3 0 −ω1

−ω2 ω1 0

=1

2 sin θ(R−RT

). (3.57)

Recall that physically ω represents the axis of rotation for the given R. Becauseof the sin θ term in the denominator problems occur when θ = kπ, where k isan integer; we address this situation next, but for now let us find the angle ofrotation θ given the rotation axis ω. Note that the sum of the diagonals of R,called the trace of R and denoted by trR, is given by

trR = r11 + r22 + r33 = 1 + 2 cos θ. (3.58)

Given any θ satisfying 1 + 2 cos θ = trR such that θ 6= 0,±π,±2π, . . . R can beexpressed as the exponential e[ω]θ.

Let us now return to the case θ = kπ, where k is some integer. When k isan even integer (corresponding to θ = 0,±2π,±4π, . . .), tr R = 3, or R = I,and it follows straightforwardly that θ = 0 is the only possible solution. Whenk is an odd integer (corresponding to θ = ±π,±3π, . . ., which in turn impliestrR = −1), the exponential formula (3.53) simplifies to

R = e[ω]π = I + 2[ω]2. (3.59)

Since [ω]2 is symmetric, it follows from the above formula that any R ∈ SO(3)for which tr R = −1 is always symmetric. Examining the three diagonal termsof the above equation and noting that ‖ω‖ = 1 leads to

ω2i =

rii + 12

, i = 1, 2, 3, (3.60)

where rii denotes the i-th diagonal entry of R. The off-diagonal terms lead tothe following three equations:

2ω1ω2 = r12

2ω2ω3 = r23

2ω1ω3 = r13,(3.61)

3.1. Rotations 57

with r12 = r21, r23 = r32, r13 = r31. Once any solution ω to these equations hasbeen determined, it can be seen that R = e[ω]kπ, k = ±π,±3π,±5π, . . .. Re-stricting θ to the interval [0, π] results in the following algorithm for computingthe logarithm of a rotation matrix R ∈ SO(3):

Algorithm for Computing the Logarithm of a Rotation Matrix

Objective: Given R ∈ SO(3), find ω ∈ R3, ‖ω‖ = 1, and θ ∈ [0, π] such that

R = e[ω]θ = I + sin θ [ω] + (1− cos θ)[ω]2. (3.62)

(i) If trR = 3, then set ω = 0, θ = 0.

(ii) If tr R = −1, then set θ = π, and ω to any of the three following vectorsthat is nonzero:

ω =1√

2(1 + r33)

r13

r23

1 + r33

(3.63)

or

ω =1√

2(1 + r22)

r12

1 + r22

r32

(3.64)

or

ω =1√

2(1 + r11)

1 + r11

r21

r31

. (3.65)

(iii) Otherwise set θ = cos−1(

tr R−12

)∈ [0, π) and [ω] = 1

2 sin θ (R−RT ).

The formula for the logarithm suggests a picture of the rotation group SO(3)as a solid ball of radius π (see Figure 3.5): given a vector v ∈ R3, let ω = v/‖v‖and θ = ‖v‖. The rotation matrix corresponding to the given v can then beregarded as a rotation about the axis ω by an angle θ. For any R ∈ SO(3) suchthat trR 6= −1, there exists a unique v inside the solid ball such that e[v] = R.When tr R = −1, its logarithm logR is given by two antipodal points on thesurface of this solid ball, lying along the corresponding rotation axis.

3.1.4 Unit Quaternions

One disadvantage with the exponential coordinates on SO(3) is that for smallrotation angles θ, the formula for the logarithm can become quite sensitivenumerically, as a result of division by sin θ in the formula [ω] = (1/ sin θ)(R −RT ). The unit quaternions are an alternative representation of rotations thatovercomes these numerical difficulties, at the cost of introducing an additionalfourth parameter. We now illustrate the definition and use of these coordinates;for further details the reader is referred to, e.g., [20], [32].


ω

π−π

^

θ

Figure 3.5: SO(3) as a solid ball of radius π.

Let R ∈ SO(3) have exponential coordinate representation R = e[ω]θ, whereas usual ‖ω‖ = 1 and θ ∈ [0, π]. The unit quaternion representation of R isconstructed as follows. Define q ∈ R4 according to

q =

q0

q1

q2

q3

=[

cos θ2ω sin θ

2

]∈ R4. (3.66)

Clearly from the above definition ‖q‖ = 1. Geometrically, q is a point lyingon the three-dimensional unit sphere in R4, denoted S3; for this reason theunit quaternions are also associated with the three-sphere S3. Naturally of thefour coordinates of q, only three can be chosen independently. Recalling that1 + 2 cos θ = tr R, and using the cosine double angle formula, i.e., cos 2u =2 cos2 u− 1, the elements of q can be obtained directly from the entries of R asfollows:

q0 = 12

√1 + r11 + r22 + r33 q1

q2

q3

= 14q0

r32 − r23

r13 − r31

r21 − 212

. (3.67)

Going the other way, given a unit quaternion (q0, q1, q2, q3), the correspond-ing rotation matrix R is obtained as a rotation about the unit axis in the direc-tion of (q1, q2, q3), by an angle 2 cos−1 q0. Explicitly,

R =

q20 + q2

1 − q22 − q2

3 2(q1q2 − q0q3) 2(q0q2 + q1q3)2(q0q3 + q1q2) q2

0 − q21 + q2

2 − q23 2(q2q3 − q0q1)

2(q1q3 − q0q2) 2(q0q1 + q2q3) q20 − q2

1 − q22 + q2

3

. (3.68)

3.2. Rigid-Body Motions 59

From the above explicit formula it is not difficult to see that both q ∈ S3 andits antipodal point −q ∈ S3 produce the same rotation matrix R; for everyrotation matrix there exists two unit quaternion representations correspondingto antipodal points on the three-sphere.

The final property of the unit quaternions concerns the product of two rota-tions. Let Rq, Rp ∈ SO(3) denote two rotation matrices, with unit quaternionrepresentations ±q,±p ∈ S3, respectively. The unit quaternion representationfor the product RqRp can then be obtained by first arranging the elements of qand p in the form of the following 2× 2 complex matrices:

Q =[

q0 + iq1 q2 + ip3

−q2 + iq3 q0 − iq1

], P =

[p0 + ip1 p2 + ip3

−p2 + ip3 p0 − ip1

], (3.69)

Now take the product N = QP , where the entries of N are written

N =[

n0 + in1 n2 + in3

−n2 + in3 n0 − in1

]. (3.70)

The unit quaternion corresponding to the productRqRp is then given by±(n0, n1, n2, n3)obtained from the entries of N :

Verification of this formula is left as an exercise at the end of this chapter.

3.2 Rigid-Body Motions

In the previous chapter we saw that the position and orientation of a rigidbody can be described by an SE(3) matrix, where SE(3) denotes the SpecialEuclidean group. SE(3) matrices go by a variety of other names, e.g., rigidbody motions, homogeneous transformations, and spatial displacements. Inthis section we first cover the basic definition and mathematical properties ofSE(3) matrices. We then discuss how SE(3) matrices are interpreted and usedin different physical contexts. The section concludes with a discussion of thesix-dimensional exponential coordinate representation of SE(3) matrices (analo-gous to the three-dimensional exponential coordinate representation of rotationmatrices), which we show can be identified with the classical screw theory.

3.2.1 Definition and Properties

We repeat here the definition of the Special Euclidean Group SE(3):

Definition 3.3. The Special Euclidean Group SE(3) is the set of all 4× 4real matrices T of the form

T =[R p0 1

], (3.71)

where R ∈ SO(3), p ∈ R3, and 0 denotes the three-dimensional row vector ofzeros. T will also sometimes be written more compactly as T = (R, p).


From the definition it should be apparent that SE(3) is of dimension six,i.e., six coordinates are needed to parametrize SE(3). The most obvious choiceof coordinates would be to use any of the earlier three-parameter representa-tions for SO(3) (e.g., Euler angles, exponential coordinates) to parametrize theorientation R, and the usual three Cartesian coordinates in R3 to parametrizethe position p. As we shall see later, a better choice is to use a six-dimensionalversion of exponential coordinates on SE(3).

The following two properties of SE(3) can be verified by calculation.

Proposition 3.9. The inverse of an SE(3) matrix always exists, and has thefollowing explicit form:[

R p0 1

]−1

=[RT −RT p0 1

]. (3.72)

Proposition 3.10. The product of two SE(3) matrices is also an SE(3) matrix.

Before stating the next proposition, we introduce the following notation:

Definition 3.4. Given T = (R, p) ∈ SE(3) and x ∈ R3, the product Tx ∈ R3

is then defined to beTx = Rx+ p. (3.73)

The above is a slight abuse of notation, but is motivated by the fact that ifx ∈ R3 is turned into a four-dimensional vector by appending a ‘1’, then[

R p0 1

] [x1

]=[Rx+ p

1

]. (3.74)

With the above definition of Tx, the next proposition can now be verifiedby calculation:

Proposition 3.11. Given T = (R, p) ∈ SE(3) and x, y ∈ R3, the followinghold:

(i) ‖Tx−Ty‖ = ‖x− y‖, where ‖ · ‖ denotes the standard Euclidean norm inR3, i.e., ‖x‖ =

√xTx.

(ii) 〈Tx−Tz, Ty−Tz〉 = 〈x− z, y− z〉 for all z ∈ R3, where 〈·, ·〉 denotes thestandard Euclidean inner product in R3, i.e., 〈x, y〉 = xT y.

In the above proposition, T is regarded as a transformation on points in R3,i.e., T transforms a point x to Tx. The first property then asserts that T pre-serves distances, while the second asserts that T preserves angles. Specifically,if x, y, z ∈ R3 represent the three vertices of a triangle, then the triangle formedby the transformed vertices {Tx, Ty, Tz} has the same set of lengths and anglesas those of the triangle {x, y, z} (or, the two triangles are said to be isometric).One can easily imagine taking x to be the points on a rigid body, in which caseTx results in a displaced version of the rigid body. It is in this sense that SE(3)is also referred to as the rigid body motions.


Figure 3.6: Expressing a vector ~v in terms of two different reference frames.

3.2.2 Physical Interpretation

In what follows rigid bodies are assumed to move in three-dimensional physicalspace, and that a length scale for physical space has been established. Givensome reference frame, a point in physical space can be represented with respectto this reference frame by a vector p ∈ R3; the vector p here is properly regardedas a representation of a point.

Such a vector is to be distinguished from a vector ~v in physical space, with adefined direction and magnitude, but that can otherwise be positioned anywherein space; such a vector is sometimes referred to as a free vector, and we shallalso adopt this terminology. Given two reference frames {a} and {b} in physicalspace, let va, vb ∈ R3 denote representations of ~v with respect to these twoframes; that is, va and vb are obtained by placing the base of ~v at the origin offrames {a} and {b}, respectively, and expressing v in terms of the given referenceframe (see Figure 3.6). In terms of the unit axes of {a} and {b}, ~v can be writtenas follows:

~v = va1xa + va2ya + va3za (3.75)= vb1xb + vb2yb + vb3zb, (3.76)

or, letting va = (va1, va2, va3), vb = (vb1, vb2, vb3),

~v =[xa ya za

]va (3.77)

=[xb yb zb

]vb. (3.78)

Now recall from our earlier discussion of rotation matrices that the unit axes of{a} and {b} are related by[

xa ya za]

=[xb yb zb

]Rba, (3.79)

from which it follows that Rbava = vb:

Proposition 3.12. Given a vector ~v in three-dimensional physical space, letva ∈ R3 and vb ∈ R3 denote representations of ~v in terms of reference frames


Figure 3.7: Three reference frames in space.

{a} and {b}, respectively. Then the following relationships among va, vb, andRab, Rba ∈ SO(3) always hold:

Rbava = vb (3.80)Rabvb = va. (3.81)

With the above proposition in hand, now consider three reference frames{a}, {b}, and {c} in physical space, and denote by Tab ∈ SE(3) the positionand orientation of frame {b} as seen from frame {a}; Tbc and Tac are definedsimilarly. Define ~u to be the vector from the origin of frame {a} to the originof frame {b}; ~v and ~w are further defined as indicated in the figure.

Proposition 3.13. Given Tab, Tbc, Tac ∈ SE(3), where

Tab =[Rab pab0 1

], Tbc =

[Rbc pbc0 1

], Tac =

[Rac pac0 1

].

Then the following relationship among Tab, Tbc, and Tac always holds:

TabTbc = Tac. (3.82)

To prove this proposition, note that pab ∈ R3 is the representation of ~u in {a}frame coordinates; similarly, pac ∈ R3 is the representation of ~w in {a} framecoordinates, while pbc ∈ R3 is the representation of ~v in {b} frame coordinates.In order to express the vector relation ~u+ ~v = ~w in {a} frame coordinates, wefirst need to find an expression for ~v in {a} frame coordinates. This can be donewith the aid of Proposition 3.12, and is simply Rabpbc. It now follows that

pac = Rabpbc + pab. (3.83)

The above, together with the relation Rac = RabRbc, can be combined into asingle matrix equation as follows:[

Rab pab0 1

] [Rbc pbc0 1

]=[Rac pac0 1

], (3.84)


Figure 3.8: Tba as a rigid displacement of a rigid body from an initial to a finalconfiguration.

or equivalently, TabTbc = Tac. �The Tab notation is a very convenient way of keeping track of the relation-

ships between multiple reference frames. The following is a direct consequenceof the previously established property:

Proposition 3.14. Taa = I, and T−1ab = Tba.

Referring again to Figure 3.7, we now change our perspective slightly, andlet q denote the point in physical space corresponding to the {c} frame origin.Let qb ∈ R3 be the vector representation of the point q in {b} frame coordinates;then clearly qb = pbc according to our earlier notation. Similarly, let qa ∈ R3

be the vector representation of the same point q in {a} frame coordinates;again, it should be clear that qa = pac. Noting that pac and pbc are related byEquation 3.83, the same relationship also exists between qa and qb:[

qa1

]=[Rab pab0 1

] [qb1

]. (3.85)

We have in fact just proven the following:

Proposition 3.15. Given a point q in physical space, let qa ∈ R3 and qb ∈ R3

denote its coordinates in terms of reference frames {a} and {b}, respectively.Then

qa = Tabqb, (3.86)

with the product Tabqb interpreted in the sense of the right-hand side of Equa-tion (3.85).

There is yet another distinct interpretation of Tba, this time as a displace-ment acting on a rigid body. Figure 3.8 shows a rigid body that has beendisplaced from some initial configuration to a new configuration by some rigidbody motion Tba. Denote the fixed frame by {a}, and let p denote a point onthis rigid body. In terms of the fixed frame, pa ∈ R3 are the coordinates for p inthe intial configuration, while pb ∈ R3 are the coordinates for p in the displacedconfiguration. By definition we then have

pb = Tbapa. (3.87)


Figure 3.9: Determining the relative location of the object from the robot ma-nipulator tip frame.

Figure 3.8 illustrates how Tba can be obtained from the initial and displacedconfigurations of the rigid body: a frame {b} is drawn such that its locationrelative to the initial rigid body configuration is the same as the location offrame {a} relative to the displaced rigid body configuration.

As a simple example of how SE(3) matrices are manipulated, consider thesetting shown in Figure 3.9, in which the robot manipulator base, table, andcamera are attached to fixed locations in the environment. The location of therobot tip frame {b} relative to the fixed frame {a}, Tab, is known (it can becalculated from the robot’s joint values). The transformations Tad and Tcd arealso known a priori. We further assume Tce can be calculated from camerameasurements of the object. In order for the robot to grasp and manipulate theobject, Tbe must be known. From

TabTbe = TadTdcTce, (3.88)

it follows thatTbe = T−1

ab TadTdcTce. (3.89)

3.2.3 Screw Motions

A classical result in kinematics that will have surprisingly far-reaching implica-tions is the Chasles-Mozzi Theorem. This result states that every rigid bodymotion can be expressed as a rotation about some fixed axis in space, followedby a pure translation parallel to that axis. In fact, the order of the rotationand translation turns out to be irrelevant: performing the translation beforethe rotation still results in the same physical displacement. Imagining that the


Figure 3.10: A general rigid body motion in three-dimensional space expressedas a screw motion with axis ω and pitch h = d/θ.

rotation and translation now occur simultaneously, one can easily visualize thiscombined motion to be the familiar motion of a screw. In this section we shallstudy the mathematical representation of screw motions.

Let us see how screw motions in three-dimensional space can be representedmathematically. Figure 3.10 shows a rigid body undergoing a displacement inthree-dimensional space; all vectors are expressed in terms of the {0} fixed framecoordinates. The initial and final configurations of the rigid body are labelled bythe frames {1} and {2}, respectively. According to the Chasles-Mozzi Theorem,there exists a screw axis, labelled ω in the figure and passing through the pointq, such that the displacement can be characterized as a screw motion about thisaxis. We now derive the homogeneous transformation corresponding to thisscrew motion. Suppose the relative displacements T01 and T02 are given by

T01 =[R p0 1

](3.90)

T02 =[R′ p′

0 1

]. (3.91)

The pitch of the screw, denoted h, is defined by the scalar quantity

h =d

θ. (3.92)

From the figure it should be apparent that

R′ = e[ω]θR (3.93)p′ = q + e[ω]θ(p− q) + hθω. (3.94)


The first equation is a consequence of the fact that the orientation of frame {2}is obtained by rotating frame {1} about the ω axis by an angle θ. The secondequation follows by verifying that p′ is the vectorial sum of the three vectors q,e[ω]θ(p − q), and hθω as indicated in the figure. The above two equations canbe combined into the following matrix equation:[

R′ p′

0 1

]=[e[ω]θ

(I − e[ω]θ

)q + hθω

0 1

] [R p0 1

]. (3.95)

The matrix [e[ω]θ

(I − e[ω]θ

)q + hθω

0 1

](3.96)

is an SE(3) matrix representation of a screw motion. Later we shall makethe case that it is far more convenient to express this matrix as a pure matrixexponential, in the same way that a rotation matrix can be expressed as theexponential of a skew-symmetric matrix.

In the meantime we first introduce some notation.

Definition 3.5. Given ω, v ∈ R3, let

S =[ωv

]∈ R6, (3.97)

which we also sometimes write more compactly as S = (ω, v) ∈ R6. Define [S]to be the following 4× 4 matrix:

[S] =[

[ω] v0 0

], [ω] =

0 −ω3 ω2

ω3 0 −ω1

−ω2 ω1 0

, (3.98)

where the bottom row of [S] consists of all zeroes.

With the above notation, let us now derive a closed-form expression for thematrix exponential e[S]θ, where S = (ω, v) with ω ∈ R3 satisfying ‖ω‖ = 1.By an appropriate choice of v, the exponential e[S]θ can be made equal to thematrix of Equation (3.96); we leave this task for later. Expanding the matrixexponential in series form leads to

e[S]θ =[e[ω]θ G(θ)v

0 1

], G(θ) = Iθ + [ω]

θ2

2!+ [ω]2

θ3

3!+ . . . (3.99)

Noting the similarity between G(θ) and the series definition for e[ω]θ, it is tempt-ing to write I + G(θ)[ω] = e[ω]θ, and to conclude that G(θ) = (e[ω]θ − I)[ω]−1.This formula is incorrect: [ω]−1 does not exist (try computing det[ω]).

Instead we make use of the result [ω]3 = −[ω] that was obtained from theCayley-Hamilton Theorem, together with the exponential formula for rotation


matrices. In this case G(θ) can be simplified to

G(θ) = Iθ + [ω] θ2

2! + [ω]2 θ3

3! + . . .

= Iθ − [ω]3 θ2

2! − [ω]4 θ3

3! − . . .= Iθ − [ω]

([ω]2 θ

2

2! + [ω]3 θ3

3! + . . .)

= Iθ − [ω](e[ω]θ − I − [ω]θ

)= Iθ + (1− cos θ)[ω] + (θ − sin θ)[ω]2.

(3.100)

Putting everything together,

Proposition 3.16. Let ω, v ∈ R3 with ω satisfying ‖ω‖ = 1, and define S =(ω, v). Then for any θ ∈ R,

e[S]θ =[e[ω]θ

(Iθ + (1− cos θ)[ω] + (θ − sin θ)[ω]2

)v

0 1

]. (3.101)

We now answer the original question of what choice of v results in (3.101)equaling (3.96). The answer is v = −[ω]q + hω. We leave the details of thisverification to the reader, but the relations [ω]ω = 0 and [ω]3 = −[ω] are helpfulfor this purpose. With this choice of v, the pitch h of the screw motion can beobtained straightforwardly as

h = ωT v. (3.102)

The above derivation essentially provides a constructive proof of the Chasles-Mozzi Theorem. That is, given an arbitrary (R, p) ∈ SE(3), one can always findsome S = (ω, v) with ‖ω‖ = 1, and a scalar θ such that

e[S]θ =[R p0 1

]. (3.103)

In the simplest case, if R = I, then ω = 0, and the preferred choice for v isv = p/‖p‖ (this would make θ = ‖p‖ the translation distance). As verification,from the series expansion

exp([

0 v0 0

]θ

)=[I vθ0 1

]. (3.104)

If trR 6= −1, the solution is given by

[ω] =1

2 sin θ(R−RT ) (3.105)

v = G−1(θ)p, (3.106)

where θ satisfies 1 + 2 cos θ = tr R. We leave as an exercise the verification ofthe following formula for G−1(θ):

G−1(θ) =1θI +

12

[ω] + (1θ− 1

2cot

θ

2)[ω]2. (3.107)

Finally, in the case when trR = −1, we choose θ = π, and [ω] can be obtained viathe formula for the logarithm on SO(3). Once [ω] and θ have been determined,v can be then obtained as v = G−1(θ)p.


Figure 3.11: Transformation of the screw motion parameters under a change ofreference frames.

Algorithm for Computing the Logarithm of an SE(3) Matrix

Objective: Given (R, p) ∈ SE(3), find S = (ω, v) ∈ R6 with ‖ω‖ = 1, andθ ∈ [0, π], such that e[ω]θ = R.

(i) If trR = 3, then set ω = 0, v = p/‖p‖, and θ = ‖p‖.

(ii) If tr R = −1, then set θ = π, and [ω] = logR as determined by the logformula on SO(3) for the case trR = −1.

(iii) Otherwise set θ = cos−1(

tr R−12

)∈ [0, π) and

[ω] =1

2 sin θ(R−RT ) (3.108)

v = G−1(θ)p, (3.109)

where G−1(θ) is given by Equation (3.107).

We now examine how the parameters of a screw motion transform under achange of reference frames. For this purpose consider the general screw motionof pitch h (see Figure 3.11). Pick an arbitrary point q on this screw axis, anddenote the coordinates in terms of reference frames {a} and {b} respectivelyby qa, qb ∈ R3. Let the screw parameters for this screw motion as seen fromframe {a} be given by Sa = (ωa, va), where va = −ωa × qa + hωa. Similarly,let the screw parameters as seen from frame {b} be Sb = (ωb, vb), where vb =−ωb × qb + hωb. Given the transform Tba = (Rba, pba) ∈ SE(3), let us attemptto express (ωb, vb) in terms of (ωa, va) and (Rba, pba).


From our previous results ωb = Rbaωa and qb = Tbaqa = Rbaqa + pba. Itfollows that

vb = −Rbaωa × (Rbaqa + pba) + hRbaωa= −[Rbaωa]Rbaqa − [Rbaωa]pba + hRbaωa= −Rba[ωa]RTbaRbaqa + [pba]Rbaωa + hRbaωa= −Rba[ωa]qa +Rbahωa + [pba]Rbaωa= Rba(−[ωa]qa + hωa) + [pba]Rbaωa= Rbava + [pba]Rbaωa,

(3.110)

where we have made use of the properties u× v = [u]v and R[u]RT = [Ru] forany u, v ∈ R3 and R ∈ SO(3). These equations for ωb and vb can be written inthe equivalent matrix form[

[ωb] vb0 0

]=[Rba pba0 1

] [[ωa] va

0 0

] [RTba −RTbapba0 1

]. (3.111)

The above is just an explicit statement of the relation [Sb] = Tba[Sa]T−1ba , which

can also be written in vector form as[ωbvb

]=[

Rba 0[pba]Rba Rba

] [ωava

]. (3.112)

We introduce the following mapping to express the above relation:

Definition 3.6. Given S = (ω, v) ∈ R6, S ′ = (ω′, v′) ∈ R6, T = (R, p) ∈SE(3), the Adjoint map S ′ = AdT (S) is defined as follows:

S‘ =[ω′

v′

]=[

R 0[p]R R

] [ωv

]= AdT (S). (3.113)

The notation [AdT ] is used to denote the 6× 6 matrix representation of AdT :

[AdT ] =[

R 0[p]R R

]. (3.114)

The adjoint map can also be expressed in the following equivalent matrix form:

[S ′] = T [S]T−1 (3.115)

=[R[ω]RT [p]Rω +Rv

0 0

]. (3.116)

The following properties of the adjoint map can be established via directcalculation:

Proposition 3.17. Let T1, T2 ∈ SE(3), and S = (ω, v). Then

AdT1 (AdT2(S)) = AdT1T2(S). (3.117)

For any T ∈ SE(3),Ad−1

T = AdT−1 , (3.118)


The second property follows from the first by choosing T1 = T−1 and T2 = T ,so that

AdT−1 (AdT (S)) = AdT−1T (S) = AdI(S) = S. (3.119)

The transformation rule for screw vectors under a change of reference framesis now summarized in the following proposition.

Proposition 3.18. Suppose a screw motion axis is described in terms of refer-ence frame {a} by the screw vector Sa = (ωa, va). The same screw motion axisis also described in terms of another reference frame {b} by the screw vectorSb = (ωb, vb). Sa and Sb are then related by

Sb = AdTba(Sa) (3.120)Sa = AdTab(Sb). (3.121)

We close this section by returning to the original motivating problem forscrew motions, Figure 3.10. The relationship between T01 and T02 can be ex-pressed as follows:

T02 = e[S]θT01 (3.122)e[S]θ = T01T12T

−101 . (3.123)

T12 can also be expressed as the matrix exponential T12 = e[S′]θ for some S ′ =(ω′, v′), and θ is the same value as that in (3.122). The latter follows by notingthat the screw angle is an intrinsic property of screw motions that is independentof any choice of reference frames; in light of this fact we could just as easily havechosen frame {1} to be the same as frame {0}, and θ would be the same in eithercase. Continuing,

e[S]θ = T01e[S′]θT−1

01 (3.124)

= eT01[S′θ]T−101 , (3.125)

which follows from the general matrix identity PeAP−1 = ePAP−1

. Using ouradjoint notation, the relation between the screws S and S ′ can therefore beexpressed as

S = AdT01(S ′). (3.126)

3.3 Summary

• Rigid bodies occupy a three-dimensional physical space. Assuming alength scale for physical space has been chosen, and given a fixed right-handed reference frame (the fixed frame), and a right-handed referenceframe (the body frame) attached to the body, the orientation of thebody frame with respect to the fixed frame is described by a 3 × 3 rota-tion matrix, while the position of the body frame origin is described by athree-dimensional vector.

3.3. Summary 71

• The set of all rotation matricees is called the Special Orthogonal group,and denoted SO(3). SO(3) matrices are orthogonal, e.g., R ∈ SO(3)satisfies RTR = I, and further satisfy detR = 1. Some other propertiesof R are R−1 = RT and ‖Rx‖ = ‖x‖ for all x ∈ R3.

• Define Rab ∈ SO(3) to be the relative orientation of frame {b} as seenfrom frame {a}. Then given three frames {a}, {b}, {c}, Rab, Rbc, and Racalways satisfy RabRbc = Rac. A consequence of this result is Raa = I andR−1ab = Rba.

• The ZYX Euler angles are a three-parameter representation for rota-tions. Given α, γ ∈ [0, 2π] and β ∈ [−π/2, π/2], the corresponding rotationmatrix R is given by

R(α, β, γ)



1 0 00 cγ −sγ0 sγ cγ

.Given R ∈ SO(3), closed-form formulas for determining (α, β, γ) are alsoavailable.

• ZYX Euler angles admit two possible physical interpretations: (i) as asequence of rotations about the axes of the moving frame (i.e., a rotationabout the z-axis of the moving frame by α, followed by a rotation aboutthe y-axis of the moving frame by β, followed by a rotation about thex-axis of the moving frame by γ); (ii) as a sequence of rotations about theaxes of the fixed frame (i.e., a rotation about the X-axis of the movingframe by γ, followed by a rotation about the Y -axis of the fixed frame byβ, followed by a rotation about the Z-axis of the fixed frame by α).

• Euler angles can be defined about any three axes, in which the first andsecond axes are orthonormal, and the second and third axes are orthonor-mal (e.g., ZYZ, ZXZ Euler angles).

• The exponential coordinates parametrize a rotation matrix R in termsof a unit vector ω ∈ R3, ‖ω‖ = 1 indicating the direction of the axis ofrotation, and a rotation angle θ ∈ [0, π]. The corresponding rotation isgiven by

e[ω]θ = I + sin θ [ω] + (1− cos θ)[ω]2.

Going the other way, given R ∈ SO(3), closed-form formulas for ω and θexist.

• The exponential coordinates suggest a visualization of SO(3) as a solidball of radius π; a point p inside this solid ball represents a rotation aboutthe axis ω = p/‖p‖, by an angle ‖p‖. If ‖p‖ = π, then p and −p representthe same rotation.


• The unit quaternions are a four-parameter representation for rotations.Given R ∈ SO(3) with exponential representation e[ω]θ, the corrrespond-ing unit quaternion representation for R is given by the following four-dimensional vector q of unit length:

q0

q1

q2

q3

= ±

12

√1 + r11 + r22 + r33

14q0

(r32 − r23)1

4q0(r13 − r31)

14q0

(r21 − 212)

.For every rotation R ∈ SO(3), there exist two unit quaternion rotations(q and −q) corresponding to antipodal points on the unit three-sphere S3.

• A rigid body motion is described by an element of the Special Euclideangroup SE(3):

T =[R p0 1

], R ∈ SO(3), p ∈ R3.

Elements of SE(3) are also represented more compactly as (R, p).

• T = (R, p) ∈ SE(3) can be interpreted as a description of the positionand orientation of some reference frame relative to another. In this case, ifTab ∈ SE(3) denotes the relative position and orientation of frame {b} asseen from frame {a}, and Tbc, Tac are similarly defined, then TabTbc = Tac.

• T = (R, p) ∈ SE(3) can also be interpreted as a coordinate transformationof a point between two reference frames. That is, if pa ∈ R3 are thecoordinates of a given point in physical space with respect to frame {a},and pb ∈ R3 are the coordinates for the same given point in physical spacewith respect to another frame {b}, then pa = Tabpb and pb = Tbapa.

• T = (R, p) ∈ SE(3) can also be interpreted as a transformation T :R3 → R3 that maps a point x ∈ R3 in physical space to another pointTx ∈ R3 in physical space, where Tx = Rx + p. The transformation Tis a rigid body motion (i.e., a displacement of a rigid body in physicalspace) in the following sense: (i) ‖Tx − Ty‖ = ‖x − y‖ for all x, y ∈ R3;(ii) 〈Tx − Tz, Ty − Tz〉 = 〈x − z, y − z〉 for all x, y, z ∈ R3, where 〈·, ·〉denotes the standard Euclidean inner product in R3, i.e., 〈x, y〉 = xT y.The former condition implies that distances are preserved, while the latterimplies that angles are preserved.

• The Chasles-Mozzi Theorem asserts that every rigid body motion T ∈SE(3) admits a representation as a screw motion, i.e., a rotation aboutsome fixed axis in space (the screw axis), followed by a translation parallelto that axis.

• In terms of some given fixed frame coordinates, the screw axis is describedby a unit vector ω ∈ R3, ‖ω‖ = 1 indicating the direction of the screw

3.4. Notes and References 73

axis, a point q ∈ R3 on the axis, and a nonnegative scalar h indicating thepitch of the screw (that is, the amount of translation per rotation θ). Inthe case of a pure rotation h = 0, while for pure translations h is regardedto be infinite. Defining v = −ω×q+hω, the rigid body motion T ∈ SE(3)corresponding to a rotation θ about the the screw S = (ω, v) is given by

e[S]θ =[e[ω]θ

(Iθ + (1− cos θ)[ω] + (θ − sin θ)[ω]2

)v

0 1

].

where the notation [S] indicates

[S] =[

[ω] v0 0

].

Given T ∈ SE(3), closed form formulas are available for finding S = (ω, v)and θ ∈ R, such that T = e[S]θ.

• Given a screw vector Sa = (ωa, va) and Tba = (Rba, pba) ∈ SE(3), theAdjoint map AdTba is defined as follows:

AdTba(Sa) =[

Rba 0[pba]Rba Rba

] [ωava

].

Denoting AdTba(Sa) = Sb, then Sa can be physically interpreted to bethe screw vector description in terms of reference frame {a}, while Sb isthe screw vector description of the same screw motion in terms of anotherreference frame {b}.

3.4 Notes and References

The ZYX Euler angle representation for rotations is also known as the roll-pitch-yaw angles, while the exponential coordinates for rotations are also known as theEuler angle-axis parameters. More detailed coverage of the various parametriza-tions of SO(3) can be found in, e.g., [32] and the references cited there.

The treatment of the matrix exponential representation for screw motions isbased on the work of Brockett [4], and a more mathematically detailed discussioncan be found in [25]. In particular, some treatments (including [25]) makea distinction between screws and twists: a screw motion is expressed as theexponential of a twist (ωθ, vθ), ‖ω‖ = 1, while the corresponding screw motionis characterized by the attributes of its screw axis ω, pitch h, and magnitudeθ. Classical screw theory is presented in its original form in R. Ball’s originaltreatise [2]. More modern (algebraic and geometric) treatments can be foundin, e.g., Bottema and Roth [3], Angeles [1], and McCarthy [20].


3.5 Exercises

1. Given a fixed frame {X, Y , Z} for physical space, let p be a point whose co-ordinates are ( 1√

3,− 1√

6, 1√

2). Suppose p is rotated about the fixed frame X-axis

by 30 degrees, then about the fixed frame Y -axis by 135 degrees, and finallyabout the fixed frame Z-axis by -120 degrees.(a) What are the coordinates of the point p following these three rotations?(b) Find the rotation matrix R such that Rp are the coordinates you obtainedin (a).

2. (a) Derive a procedure for finding the ZXZ Euler angles of a rotation matrix.(b) Using the results of (a), find the ZXZ Euler angles for the following rotationmatrix: −

1√2

1√2

0− 1

2 − 12

1√2

12

12

1√2

.

3. Show that rotation matrices of the form r11 r12 0r21 r22 r23

r31 r32 r33

can be parametrized using just two parameters θ and φ as follows: cos θ − sin θ 0

sin θ cosφ cos θ cosφ − sinφsin θ sinφ cos θ sinφ cosφ

.What should the range of values be for θ and φ?

4. (a) Show that the three eigenvalues of a rotation matrix R ∈ SO(3) each haveunit magnitude, and conclude that they can always be written {µ+iν, µ−iν, 1},where µ2 + ν2 = 1.(b) Show that a rotation matrix R ∈ SO(3) can always be factored in the form

R = A

µ ν 0−ν µ 0

0 0 1

A−1,

where A ∈ SO(3) and µ2 + ν2 = 1. (Hint: Denote the eigenvector associatedwith the eigenvalue µ+ iν by x+ iy, x, y ∈ R3, and the eigenvector associatedwith the eigenvalue 1 by z ∈ R3. For the purposes of this problem you may

3.5. Exercises 75

assume that the set of vectors {x, y, z} can always be chosen to be linearlyindependent.)

5. (a) Find the general solution to the differential equation x = Ax, where

A =[−2 10 −1

].

What happens to the solution x(t) as t→∞?(b) Do the same for

A =[

2 −11 2

].

What happens to the solution x(t) as t→∞?

6. Let x ∈ R2, A ∈ R2×2, and consider the linear differential equation x(t) =Ax(t). Suppose that

x(t) =[

e−3t

−3e−3t

]is a solution for the initial condition x(0) = (1,−3), and

x(t) =[et

et

].

is a solution for the initial condition x(0) = (1, 1). Find A and eAt.

7. Given a differential equation of the form x = Ax + f(t), where x ∈ Rn andf(t) is a given differentiable function of t. Show that the general solution canbe written

x(t) = eAtx(0) =∫ t

0

eA(t−s)f(s) ds.

(Hint: Define z(t) = e−Atx(t), and evaluate z(t).)

8. (a) Prove the matrix identity MeAM−1 = eMAM−1.

(b) Under what conditions on A,B ∈ Rn×n is the following matrix identitytrue?

eAeB = eA+B .

9. Consider a wrist mechanism with two revolute joints θ1 and θ2, in which theend-effector frame orientation R ∈ SO(3) is given by

R = e[w1]θ1e[w2]θ2 ,

with ω1 = (0, 0, 1) and ω2 = (0, 1√2,− 1√

2). Determine whether the following

orientation is reachable (that is, find, if it exists, a solution (θ1, θ2) for the


following R):

R =

1√2

0 − 1√2

0 1 01√2

0 1√2

Figure 3.12: A three degree of freedom wrist mechanism.

10. Figure 3.12 shows a three degree of freedom wrist mechanism in its zeroposition (that is, with all its joints set to zero).(a) Express the tool frame orientation R03 = R(α, β, γ) as a product of threerotation matrices.(b) Find all possible angles (α, β, γ) for the two values of R03 given below. Ifno solution exists, explain why in terms of the analogy between SO(3) and thesolid ball of radius π.

(i) R03 =

0 1 01 0 00 0 −1

.(ii) R03 = e[w]π2 , where w = (0, 1√

5, 2√

5).

11. (a) Suppose we seek the logarithm of a rotation matrix R whose trace is-1. From the exponential formula

e[ω]θ = I + sin θ[ω] + (1− cos θ)[ω]2, ‖ω‖ = 1,

3.5. Exercises 77

and recalling that trR = −1 implies θ = π, the above equation simplifies to

R = I + 2[ω]2 =

1− 2(ω22 + ω2

3) 2ω1ω2 2ω1ω3

2ω1ω2 1− 2(ω21 + ω2

3) 2ω2ω3

2ω1ω2 2ω2ω3 1− 2(ω21 + ω2

2)

Using the fact that ω2

1 + ω22 + ω2

3 = 1, is it correct to conclude that

ω1 =

√r11 + 1

2, ω2 =

√r22 + 1

2, ω3 =

√r33 + 1

2.

is also a solution?(c) Using the fact that [ω]3 = −[ω], the identity R = I + 2[ω]2 can also bewritten in the alternative form

R− I = 2[ω]2

[ω] (R− I) = 2 [ω]3 = −2 [ω][ω] (R+ I) = 0.

The resulting equation is a system of three linear equations in (ω1, ω2, ω3). Whatis the relation between the solution to this linear system and the logarithm ofR?

12. (a) Given a rotation matrix A = Rot(z, α), where Rot(z, α) indicates arotation about the z-axis by an angle α, find all rotation matrices R ∈ SO(3)that satisfy AR = RA.(b) Given rotation matrices A = Rot(z, α) and B = Rot(z, β), with α 6= β, findall rotation matrices R ∈ SO(3) that satisfy AR = RB.(c) Given arbitrary rotation matrices A,B ∈ SO(3), find all solutions R ∈ SO(3)to the equation AR = RB.

13. (a) Exploiting all of the known properties of rotation matrices, determinethe minimum number of arithmetic operations (multiplication and division, ad-dition and subtraction) required to multiply two rotation matrices.(b) Due to finite arithmetic precision, the numerically obtained product of tworotation matrices is not necessarily a rotation matrix; that is, the resultingrotation A may not exactly satisfy ATA = I as desired. Devise an iterativenumerical procedure that takes an arbitrary matrix A ∈ R3×3, and produces amatrix R ∈ SO(3) that minimizes

‖A−R‖2 = tr (A−R)(A−R)T .

14. (a) Verify the formula for obtaining the unit quaternion representation ofa rotation R ∈ SO(3).(b) Verify the formula for obtaining the rotation matrix R given a unit quater-nion q ∈ S3.


(c) Verify the product rule for two unit quaternions; that is, given two unitquaternions q, p ∈ S3 corresponding respectively to the rotations R,Q ∈ SO(3),find a formula for the unit quaternion representation of the product RQ ∈SO(3).(d) Compare the number of arithmetic operations for multiplying two rotationmatrices versus two unit quaternions. Which requires fewer arithmetic opera-tions?

Figure 3.13: Four reference frames defined in a robot’s work environment.

15. Consider the robot of Figure 3.13, in which four reference frames aredepicted: the fixed frame {a}, the end-effector frame effector {b}, camera frame{c}, and workpiece frame {d}.(a) Find Tad and Tcd in terms of the dimensions given in the figure.(b) Find Tab given that

Tbc =

1 0 0 40 1 0 00 0 1 00 0 0 1

.

16. Consider a robot arm mounted on a spacecraft as shown in Figure 3.14,in which frames are attached to the earth {e}, satellite {s}, the spacecraft {a},and the robot arm {r}, respectively.(a) Given Tea, Tar, and Tes, find Trs.

3.5. Exercises 79

Figure 3.14: A robot arm mounted on a spacecraft.

(b) Suppose the frame {s} origin as seen from {e} is (1, 1, 1). Suppose further-more that

Ter =

−1 0 0 1

0 1 0 10 0 −1 10 0 0 1

.Write down the coordinates of the frame {s} origin as seen from frame {r}.

Figure 3.15: A classical bicycle with a larger front wheel.

17. Consider the classical bicycle of Figure 3.15, in which the diameter of thefront wheel is twice that of the rear wheel. Frames {a} and {b} are attached tothe centers of each wheel, and frame {c} is attached to the top of the front wheel.Assuming the bike moves forward in the y direction, find Tac as a function ofthe front wheel’s rotation angle θ (assume θ = 0 at the instant shown in thefigure).


Figure 3.16: A laser tracking a moving target.

18. A target moves along a circular path at constant angular velocity ω rad/secas shown in Figure 3.16. The target is tracked by a laser mounted on a movingplatform, rising vertically at constant speed v. Assume the laser and the plat-form start at L1 at t = 0, while the target starts at frame T1.(a) Derive frames T01, T12, T03 as a function of t.(b) Using your results from part (a), derive T23 as a function of t.

Figure 3.17: Spacecraft and space station.

19. Suppose the space station of Figure 18 is in circular orbit around the earth,and at the same time rotates about an axis always pointing toward the northstar. A spacecraft heading toward the space station is unable to locate thedocking port due to an instrument malfunction. An earth-based ground station

3.5. Exercises 81

sends the following information to the spacecraft:

Tab =

0 −1 0 −1001 0 0 3000 0 1 5000 0 0 1

, pa =

08000

,

where pa is the vector ~p expressed in {a} frame coordinates.(a) From the given information, find rb, the vector ~r expressed in {b} framecoordinates.(b) Determine Tbc at the instant shown in the figure. Assume here that the yand z axes of the {a} and {c} frames are coplanar with the docking port.

Figure 3.18: Two toy cars on a round table.

20. Two toy cars are moving on a round table as shown in Figure 3.18. Car 1moves at a constant speed v1 along the circumference of the table, while car 2moves at a constant speed v2 along a radius; the positions of the two vehiclesat t = 0 are shown in the figure.(a) Find T01, T02 as a function of t.(b) Find T12 as a function of t.

21. Figure 3.19 shows the configuration, at t = 0, of a robot arm whose firstjoint is a screw joint of pitch h = 2. The arm’s link lengths are L1 = 10,L2 = L3 = 5, and L4 = 3. Suppose all joint angular velocities are constant,with values ω1 = π

4 , ω2 = π8 , w3 = −π4 rad/sec. Find Tsb(4) ∈ SE(3), i.e., the

end-effector frame {b} ∈ SE(3) relative to the fixed frame {s}, at time t = 4.

22. A cube undergoes two different screw motions from frame {1} to frame {2}as shown in Figure 3.20. In both cases (a) and (b), the initial configuration ofthe cube is

T01 =

1 0 0 00 1 0 10 0 1 00 0 0 1

.


Figure 3.19: A robot arm with a screw joint.

(a) a (b) b

Figure 3.20: A cube undergoing two different screw motions.

(a) For each case (a) and (b), find the screw parameter S = (ω, v) such thatT02 = e[S]T01, where no constraints are placed on ω or v.(b) Repeat (a), this time with the constraint that ‖ω‖ ∈ [−π, π].

23. A particle starts from the origin, and undergoes a radially increasing cir-cular spiral motion in the y direction as illustrated in Figure 3.21. For everytranslation of 10 units in the y direction, the particle completes one revolution.Attaching a moving frame to the particle, the motion of this moving frame can

3.5. Exercises 83

Figure 3.21: A particle undergoing a screw motion.

be expressed in the formT (θ) = e[Sa]θe[Sb]θ,

for some Sa = (ωa, va), Sb = (ωa, vb). Find Sa and Sb.

24. Given ω ∈ R3, ‖ω‖ = 1, and θ ∈ R, show that

(Iθ + (1− cos θ)[ω] + (1− sin θ)[ω]2

)−1= I−θ

2[ω]+

(1− θ

2(sec θ + cot θ)

)[ω]2.

Under what conditions, if any, will the inverse fail to exist? (Hint: Express theinverse as a quadratic matrix polynomial in [ω], and determine the coefficients.The quadratic polynomial assumption can be justified via the identity [ω]3 =−[ω].)

25. Given two reference frames {a} and {b} in physical space, and a fixed frame{o}, define the distance between frames {a} and {b} as

dist(Toa, Tob) ≡√θ2 + ||pab||2

where Rab = e[ω]θ. Suppose the fixed frame is displaced to another frame {o′},and that To′a = SToa, To′b = STo′b for some constant S = (Rs, ps) ∈ SE(3).(a) Evaluate dist(To′a, To′b) using the above distance formula.(b) Under what conditions on S does dist(Toa, Tob) = dist(To′a, To′b)?

Chapter 4

Forward Kinematics ofOpen Chains

This chapter addresses the forward kinematics problem for open chains: givenan open chain robot arm with a prescribed end-effector frame, the goal is todetermine the end-effector frame’s position and orientation as a function of thejoint values. Two widely used forward kinematic representations for open chainswill be considered: the homogeneous transformation representation based on theDenavit-Hartenberg parameters, and the screw-theoretic formulation basedon the Product of Exponentials formula.

4.1 Denavit-Hartenberg Representation

The basic idea underlying the Denavit-Hartenberg approach to forward kine-matics is to attach reference frames to each link of the open chain, and to derivethe forward kinematics based on knowledge of the relative displacements be-tween adjacent link frames. This approach is illustrated in Figure 4.1. Assumethat a fixed reference frame has been established, and that a reference frame(the end-effector frame) has been attached to some point on the last link ofthe open chain. For a chain consisting of n one degree of freedom joints, thelinks are numbered sequentially from 0 to n, in which the ground link is labelled0, and the end-effector frame is attached to some point on link n. Referenceframes attached to the links are also correspondingly labelled from {0} to {n}.The joint variable corresponding to the i-th joint is denoted θi. The forwardkinematics of the n-link open chain can then be expressed as

T0n(θ1, . . . , θn) = T01(θ1)T12(θ2) · · ·Tn−1,n(θn), (4.1)

where Ti,i−1 ∈ SE(3) denotes the relative displacement between link frames{i − 1} and {i}. For open chains Ti−1,i is usually straightforward to obtain,leading to a systematic method for deriving the foward kinematics T0n.

85

86 Forward Kinematics of Open Chains

joint1

joint2

joint3

x0

y0

z0

x2x1

x3

y1y2

y3

z3

z2z1

link3

base (link0)

link2

link1

Figure 4.1: Assignment of link reference frames.

α i-1

x i-1^

z i-1^

y i-1^ a i-1 θ i

x i^z i

^

y i^

d i

axis i-1 axis i

Figure 4.2: Illustration of Denavit-Hartenberg parameters.

4.1.1 Assigning Link Frames

Rather than attaching reference frames to each link in some arbitrary fashion,in the Denavit-Hartenberg convention a set of rules for assigning link framesis observed. Figure 4.2 illustrates the frame assignment convention for twoadjacent revolute joints i− 1 and i that are connected by link i− 1.

The first rule is that the z-axis of the link frame always coincides with thejoint axis. In the figure, zi coincides with joint axis i, and zi−1 coincides withjoint axis i− 1. The direction of each link frame z-axis is determined such thatpositive rotations are counterclockwise about the z-axis.

Once the z-axis direction has been assigned, the next rule determines the

4.1. Denavit-Hartenberg Representation 87

origin of the link reference frame. First, find the line segment that orthogonallyintersects both joint axes zi−1 and zi. For now let us assume that this linesegment is unique; the case where it is not unique (i.e., when the two jointaxes are parallel), or fails to exist (i.e., when the two joint axes intersect), isaddressed later. Connecting joint axes i− 1 and i by a mutually perpendicularline, the origin of frame {i − 1} is then located at the point where this lineintersects joint axis i− 1.

Determining the remaining x- and y-axes of each link reference frame is nowstraightforward: the x axis is chosen to be in the direction of the mutuallyperpendicular line pointing from the i− 1 axis to the i axis. The y-axis is thenuniquely determined from the cross-product x × y = z. Figure 4.2 depicts thelink frames i and i− 1 chosen according to this convention.

Having assigned reference frames in this fashion for links i and i−1, we nowdefine four parameters that exactly specify Ti−1,i:

• The length of the mutually perpendicular line, denoted by the scalar ai−1,is called the link length of link i− 1. Despite its name, this link lengthdoes may necessarily correspond to the actual length of the physical link.

• The link twist αi−1 is the angle from zi−1 to zi, measured about xi−1.

• The link offset di is the distance from the intersection of xi−1 and zi tothe link i frame origin (the positive direction is defined to be along the ziaxis).

• The joint angle φi is the angle from xi−1 to xi, measured about thezi-axis in the right-hand sense.

The above four parameters constitute the Denavit-Hartenberg parameters.For an open chain with n one degree-of-freedom joints, the 4nDenavit-Hartenbergparameters are sufficient to completely describe the forward kinematics. In thecase of an open chain with all joints revolute, the link lengths ai−1, twists αi−1,and offset parameters di are all constant, while the joint angle parameters φiact as the joint variables.

We now consider the case where the mutually perpendicular line is undefinedor fails to be unique, as well as when some of the joints are prismatic, and finally,how to choose the ground and end-effector frames.

When Adjacent Revolute Joint Axes Intersect

If two adjacent revolute joint axes intersect each other, then the mutually per-pendicular line between the joint axes fails to exist. In this case we choose xi−1

to be perpendicular to the plane spanned by zi−1 and zi. There are two possi-bilities here, both of which are acceptable: one leads to a positive value of thetwist angle αi−1, while the other leads to a negative value.


α i-1

x i-1^

z i-1^

y i-1^

axis i-1

x i^y i

^ z i^

a i-1

θ i

axis i

d i

Figure 4.3: Link frame assignment convention for prismatic joints. Joint i − 1is a revolute joint, while joint i is a prismatic joint.

When Adjacent Revolute Joint Axes are Parallel

The second special case occurs when two adjacent revolute joint axes are par-allel. In this case there exist many possibilities for a mutually perpendicularline, all of which are valid (more precisely, a one-parameter family of mutualperpendicular lines is said to exist). Again, it is important to detail preciselyhow the link frames are assigned. A useful guide is to try to choose the mutuallyperpendicular line that is the most physically intuitive, and simplifies as manyDenavit-Hartenberg parameters as possible (e.g., such that their values becomezero).

Prismatic Joints

For prismatic joints, the z-direction of the link reference frame is chosen tobe along the positive direction of translation. In a sense this convention isconsistent with that for revolute joints, in which the z-axis indicates the positiveaxis of rotation. With this choice the link offset parameter di now becomes thejoint variable (see Figure 4.3). The procedure for choosing the link frame origin,as well as the remaining x and y-axes, remains the same as for revolute joints.


Figure 4.4: An example of three frames {a}, {b}, and {c}, in which the trans-formations Tab and Tbc cannot be described by any set of Denavit-Hartenbergparameters.

Assigning the Ground and End-Effector Frames

Our frame assignment procedure described thus far does not specify how tochoose the ground and final link frames. Here as before, a useful guideline is tochoose initial and final frames that are the most physically intuitive, and simplifyas many Denavit-Hartenberg parameters as possible. This usually implies thatthe ground frame is chosen to coincide with the link 1 frame in its zero (rest)position; in the event that the joint is revolute, this choice forces a0 = α0 =d1 = 0, while for a prismatic joint we have a0 = α0 = φ1 = 0. The end-effectorframe is typically attached to some reference point on the end-effector, and alsoassigned in a way that simplifies as many of the Denavit-Hartenberg parametersas possible (e.g., their values become zero).

It is important to realize that arbitrary choices of the ground and end-effector frames may not always be possible, since there may not exist a valid setof Denavit-Hartenberg parameters to describe the relative transformation; weelaborate on this point below.

4.1.2 Why Four Parameters are Sufficient

In our earlier study of spatial displacements, we argued that a minimum ofsix independent parameters were required to describe the relative displacementbetween two frames in space: three for the orientation, and three for the po-sition. Based on this result, it would seem that for an n-link arm, a total of6n parameters would be required to completely describe the forward kinematics(each Ti−1,i in the above equation would require six parameters). Surprisingly,in the Denavit-Hartenberg parameter representation only four parameters arerequired for each transformation Ti−1,i. Although this result may at first appearto contradict our earlier results, this reduction in the number of parameters isaccomplished by the carefully stipulated rules for assigning link reference frames.If the link reference frames had been assigned in arbitrary fashion, then moreparameters would have been required.

Consider, for example, the link frames shown in Figure 4.4. The transfor-


mation from frame {a} to frame {b} requires a translation along the y-axis offrame {a}. If one were to try to express the transformation Tab in terms ofthe Denavit-Hartenberg parameters (α, a, d, θ) as prescribed above, it shouldbecome apparent that no such set of parameter values exist; only translationsalong the x and z axes are allowed under our Denavit-Hartenberg convention.Similarly, the transformation Tac also does not admit a description in terms ofDenavit-Hartenberg parameters, as only rotations about the x- and z- axes arepermissible.

Given that the Denavit-Hartenberg convention uses exactly four parametersto describe the transformation between link frames, one might naturally wonderif the number of parameters can be reduced even further, by an even more cleverset of link frame assignment rules. Denavit and Hartenberg show that this isnot possible, and that four is the minimum number of parameters [7].

We end this section with a reminder that the convention for assigning linkframes that we have presented here is by no means a standard one. Whereas wechose the z-axis to coincide with the joint axis, some authors choose the x-axis,and reserve the z-axis to be the direction of the mutually perpendicular line. Toavoid ambiguities in the interpretation of the Denavit-Hartenberg parameters,it is de rigeur to include a concise description of the link frames together withthe parameter values.

4.1.3 Manipulator Forward Kinematics

Once all the transformations Ti−1,i between adjacent link frames are knownin terms of their Denavit-Hartenberg parameters, the forward kinematics isobtained by sequentially multiplying these link transformations. Each link frametransformation is of the form

Ti−1,i = Rot(x, αi−1) · Trans(x, ai−1) · Trans(z, di) · Rot(z, φi)

=

cosφi − sinφi 0 ai−1

sinφi cosαi−1 cosφi cosαi−1 − sinαi−1 −di sinαi−1

sinφi sinαi−1 cosφi sinαi−1 cosαi−1 di cosαi−1

0 0 0 1

,


where

Rot(x, αi−1) =

1 0 0 00 cosαi−1 − sinαi−1 00 − sinαi−1 cosαi−1 00 0 0 1

(4.2)

Trans(x, ai−1) =

1 0 0 ai−1

0 1 0 00 0 1 00 0 0 1

(4.3)

Trans(z, di) =

1 0 0 00 1 0 00 0 1 di0 0 0 1

(4.4)

Rot(z, φi) =

cosφi−1 − sinφi−1 0 0− sinφi−1 cosφi−1 0 0

0 0 1 00 0 0 1

. (4.5)

A useful way to visualize Ti,i−1 is to transport frame {i − 1} to frame {i} viathe following sequence of four transformations:

(i) Rotate frame {i− 1} about its x axis by an angle αi−1.

(ii) Translate this new frame along its x axis by a distance ai−1.

(iii) Translate this new frame along its z axis by a distance di.

(iv) Translate this new frame about its z axis by an angle φi.

Note that switching the order of the first and second steps will not change thefinal form of Ti−1,i. Similarly, the order of the third and fourth steps can alsobe switched without affecting Ti−1,i.

4.1.4 Examples

We now derive the Denavit-Hartenberg parameters for some common spatialopen chain structures.

Example: A 3R Spatial Open Chain

Consider the 3R spatial open chain of Figure 4.5, shown in its zero position(i.e., with all its joint variables set to zero). The assigned link reference framesare shown in the figure, and the corresponding Denavit-Hartenberg parameterslisted in the following table:


Figure 4.5: A 3R spatial open chain.

i αi−1 ai−1 di φi

1 0 0 0 φ1

2 90◦ L1 0 φ2 − 90◦

3 −90◦ L2 0 φ3

Note that frames {1} and {2} are uniquely specified from our frame assign-ment convention, but that we have some latitude in choosing frames {0} and{3}. Here we choose the ground frame {0} to coincide with frame {1} (resultingin α0 = a0 = d1 = 0), and frame {3} such that x3 = x2 (resulting in no offsetto the joint angle φ3).

Example: A Spatial RRRP Open Chain

The next example we consider is the four degree-of-freedom RRRP spatial openchain of Figure 4.6, also shown in its zero position. The link frame assignmentsare shown in the figure, and the corresponding Denavit-Hartenberg parameterslisted in the following table:


1 0 0 0 φ1

2 90◦ 0 0 φ2

3 0 L2 0 φ3 + 90◦

4 90◦ 0 d4 0


Figure 4.6: A RRRP spatial open chain.

Figure 4.7: A 6R spatial open chain.

Note here that the four joint variables are (φ1, φ2, φ3, d4), where d4 is thedisplacement of the prismatic joint. As in the previous example, the groundframe {0} and final link frame {4} have been chosen to make as many of theDenavit-Hartenberg parameters become zero.

Example: A Spatial 6R Elbow-Type Open Chain

The final example we consider is a widely used six 6R robot arm of the elbowtype (Figure 4.7). This open chain has six rotational joints: the first three jointsfunction as a Cartesian positioning device, while the last three joints act as a


ZYZ type wrist. The link frames are shown in the figure, and the correspondingDenavit-Hartenberg parameters are listed in the following table:


1 0 0 0 φ1

2 90◦ 0 0 φ2

3 0 L1 0 φ3 + 90◦

4 90◦ 0 L2 φ4 + 180◦

5 90◦ 0 0 φ5 + 180◦

6 90◦ 0 0 φ6

4.2 Product of Exponentials Formula

In this section we present an alternative derivation of the forward kinematics,called the product of exponentials formula, that is based on the mathematicalrepresentation for screw motions introduced in the previous chapter.

4.2.1 Formulation and Properties

Recall that every rigid motion can be expressed as a screw motion, and thata screw motion can be expressed as a matrix exponential of the form e[S]θ,S = (ω, v), where [S] ∈ R4×4 is a matrix of the form

[S] =[

[ω] v0 0

]. (4.6)

Here ω ∈ R3 is a unit vector in the direction of the screw axis and [ω] ∈ R3×3

its skew-symmetric matrix representation, v = −ω × q + hω with q ∈ R3 anarbitrary point on the screw axis and h ∈ R the screw pitch.

The key observation behind the product of exponentials formula is that eachjoint effectively applies a screw motion to the outwardly attached link. To illus-trate, consider an n-revolute joint open chain under the following assumptions:

(i) a fixed reference frame has been chosen;

(ii) an end-effector frame has been attached to some point on the final link ofthe chain;

(iii) the zero position and direction of positive rotation (or translation in thecase of a prismatic joint) have been specified for each joint axis.

Label the joint variables θ1, θ2, . . . , θn, and let M ∈ SE(3) be the homogeneoustransformation describing the end-effector frame (relative to the fixed frame)when the manipulator is in its zero position.

In the even that joints θ1, . . . , θn−1 are fixed at the zero position, and onlythe final joint θn is allowed to move, the end-effector frame moves according to

T0n = e[Sn]θnM, (4.7)

4.2. Product of Exponentials Formula 95

where Sn = (ωn, vn) are the screw parameters for the final joint: if the jointis revolute (corresponding to a screw motion of zero pitch), recall that ωn is aunit vector in the direction of joint axis n, and vn = −ωn × qn, with qn anyarbitrary point on joint axis n. If the joint is prismatic, then ω = 0, and v is aunit vector in the direction of positive translation. We remind the reader thatall vector quantities here are expressed in terms of the fixed frame {0}.

If we now allow both θn−1 and θn to rotate while keeping the other jointsfixed at zero, then it follows that

T0n = e[Sn−1]θn−1e[Sn]θnM. (4.8)

This relation holds because of the serial nature in which the joints θn−1 and θnare arranged; the e[Sn−1]θn−1 term left-multiplies the e[Sn]θn term. It is not toodifficult to see how to generalize this argument. If all joints are now allowed tomove, we have

T0n = e[S1]θ1 · · · e[Sn−1]θn−1e[Sn]θnM. (4.9)

This is the product of exponentials formula describing the forward kinematicsof an n-link open chain. Some features about this formula worth emphasizingare:

• No link reference frames need to be assigned; all that is required is a choiceof fixed and end-effector frames, together with a definition of the manip-ulator’s zero position, and (in the case of a revolute joint) the directionof positive rotation for each joint, or (in the case of a prismatic joint) thedirection of positive translation.

• If joint i is prismatic, then Si = (0, vi), where vi ∈ R3 is a unit vec-tor indicating the direction of positive translation. Unlike the case withDenavit-Hartenberg parameters, where the joint variable can be either θior di depending on whether the joint is revolute or prismatic, with theproduct of exponentials formula the joint variable is always labelled in aconsistent fashion by θi (note that this definition will in general differ fromthat of the Denavit-Hartenberg joint angle).

• The ground frame can be chosen in any arbitrary manner, and the end-effector frame can be attached in any arbitrary fashion to the final link.The choice of end-effector frame is reflected in the product of exponen-tials formula through the homogeneous transformation M ∈ SE(3), whichspecifies the location of the end-effector frame when the manipulator is inits zero position.

The fact that no link reference frames are used, together with the explicit recog-nition that it is the location of the joint axes that matter, is what makes theproduct of exponentials formula so appealing from an intuitive and modelingperspective. We shall see even further advantages of this formula when weconsider the differential kinematics.


L L L

x

y

z

{T}x

y

z

{S}

Figure 4.8: POE forward kinematics for the 6R open chain.

4.2.2 Examples

Example: A 6R Elbow-Type Spatial Open Chain

We now derive the forward kinematics of the 6R elbow-type open chain ofFigure 4.8, this time in the following product of exponentials form:

T06 = e[S1]θ1 · · · e[S6]θnM.

The zero position of the chain, together with the direction of positive rotationfor each joint, are as shown in the figure. We also choose a fixed frame {0},and attach an end-effector frame {6} to the final link as shown. With themanipulator in its zero position, the end-effector frame as seen from the fixedframe is

M =

1 0 0 00 1 0 3L0 0 1 00 0 0 1

(4.10)

The screw axis for joint 1 is ω1 = (0, 0, 1). To determine v1, we first pick anyarbitrary point q1 that lies on joint axis 1; the most convenient choice is theorigin q1 = (0, 0, 0). Since all the joints are revolute, their corresponding pitchesare zero, and hence v1 = −ω1 × q1 = (0, 0, 0). The screw axis for joint 2 is inthe y direction of the fixed frame, so ω2 = (0, 1, 0). Choosing q2 = (0, 0, 0),we have v2 = (0, 0, 0). The screw axis for joint 3 is ω3 = (−1, 0, 0). Choosingq3 = (0, 0, 0) leads to v3(0, 0, 0). The screw axis for joint 4 is ω4 = (−1, 0, 0).Choosing q4 = (0, L, 0), it follows that v4 = (0, 0,−L). The screw axis for joint5 is ω5 = (−1, 0, 0); choosing q5 = (0, 2L, 0) leads to v5 = (0, 0,−2L). The screwaxes for joint 6 is ω6 = (0, 1, 0); choose q6 = (0, 0, 0), so that v6 = (0, 0, 0). Insummary, the Si = (ωi, vi) for the 6R open chain are as follows:


Figure 4.9: The RRPRRR spatial open chain.

i ωi vi

1 ( 0, 0, 1) (0, 0, 0)2 ( 0, 1, 0) (0, 0, 0)3 (-1, 0, 0) (0, 0, 0)4 (-1, 0, 0) (0, 0, −L)5 (-1, 0, 0) (0, 0, −2L)6 ( 0, 1, 0) (0, 0, 0)

Example: A RRPRRR Spatial Open Chain

As a second example, we consider the six degree-of-freedom RRPRRR spatialopen chain of Figure 4.9, also commonly known as a Stanford-type arm. Theend-effector frame in the zero position is given by

M =

1 0 0 00 1 0 L1 + L2

0 0 1 00 0 0 1

.The values of the screw parameters Si = (ωi, vi) are listed in the following table:

i ωi vi

1 ( 0, 0, 1) (0, 0, 0)2 ( 1, 0, 0) (0, 0, 0)3 ( 0, 0, 0) (0, 1, 0)4 ( 0, 1, 0) (0, 0, 0)5 ( 1, 0, 0) (0, 0, −L1)6 ( 0, 1, 0) (0, 0, 0)

Note that the third joint is prismatic, so that ω3 = 0 and v3 is a unit vectorin the direction of positive translation.


4.2.3 Relation with the Denavit-Hartenberg Representation

The product of exponentials formula can also be derived directly from theDenavit-Hartenberg parameter-based representation of the forward kinematics.As before, denote the relative displacement between adjacent link frames by

Ti−1,i = Rot(x, αi−1) · Trans(x, ai−1) · Trans(z, di) · Rot(z, φi).

If joint i is revolute, the first three matrices can be regarded as constant, andφi becomes the revolute joint variable. Define θi = φi, and

Mi = Rot(x, αi−1) · Trans(x, ai−1) · Trans(z, di), (4.11)

and write Rot(z, θi) as the following matrix exponential:

Rot(z, θi) = e[Ai]θi , [Ai] =

0 −1 0 01 0 0 00 0 0 00 0 0 0

. (4.12)

With the above definitions we can write Ti−1,i = Mie[Ai]θi .

If joint i is prismatic , then di becomes the joint variable, φi is a constantparameter, and the order of Trans(z, di) and Rot(z, φi) in Ti−1,i can be reversed(note that reversing translations and rotations along the same axis still resultsin the same motion). In this case we can still write Ti−1,i = Mie

[Ai]θi , whereθi = di, and

Mi = Rot(x, αi−1)Trans(x, ai−1)Rot(z, φi) (4.13)

[Ai] =

0 0 0 00 0 0 00 0 0 10 0 0 0

. (4.14)

Based on the above, for an n-link open chain containing both revolute andprismatic joints, the forward kinematics can be written

T0,n = M1e[A1]θ1M2e

[A2]θ2 · · ·Mne[An]θn (4.15)

where θi denotes joint variable i, and [Ai] is either of the form (4.12) or (4.14)depending on whether joint i is revolute or prismatic.

We now make use of the matrix identity MePM−1 = eMPM−1, which holds

for any nonsingular M ∈ Rn×n and arbitrary P ∈ Rn×n. The above can also berearranged as MeP = eMPM−1

M . Beginning from the left of Equation (4.15),if we repeatedly apply this latter identity, we obtain, after n iterations, theproduct of exponentials formula as originally derived:

T0n = eM1[A1]M−11 θ1(M1M2)e[A2]θ2 · · · e[An]θn

= eM1[A1]M−11 θ1e(M1M2)[A2](M1M2)−1θ2(M1M2M3)e[A3]θ3 · · · e[An]θn

= e[S1]θ1 · · · e[Sn]θnM,(4.16)


where[Si] = (M1 · · ·Mi−1)[Ai](M1 · · ·Mi−1)−1, i = 1, . . . , n, (4.17)

and M = M1M2 · · ·Mn.Let us now re-examine the physical meaning of the Si, by recalling how screw

parameters transform under a change of reference frames. If Sa represents thescrew parameters for a given screw motion with respect to frame {a}, and Sbrepresents the screw parameters for the same physical screw motion, but thistime with respect to a frame {b}, then recall from Chapter 4 that Sa and Sb arerelated by

[Sb] = Tba[Sa]T−1ba , (4.18)

or using the adjoint notation AdTba ,

Sb = AdTba(Sa). (4.19)

Seen from the perspective of this transformation rule, Equation (4.17) suggeststhat, with the chain in its zero position, each Ai is the screw parameter for jointaxis i as seen from link frame {i}, while each Si is the screw parameter for jointaxis i as seen from the fixed frame {0}.

4.2.4 A Second Formulation

The earlier matrix identity can also be expressed as MeM−1PM = ePM . Begin-

ning from the right of our previously derived product of exponentials formula,if we repeatedly apply this identity we obtain, after n iterations,

T0n = e[S1]θ1 · · · e[Sn]θnM

= e[S1]θ1 · · ·MeM−1[Sn]Mθn

= e[S1]θ1 · · ·MeM−1[Sn−1]Mθn−1eM

−1[Sn]Mθn

= Me[B1]θ1e[B2]θ2 · · · e[Bn]θn ,

(4.20)

where each [Bi] = M−1[Si]M , i = 1, . . . , n. This is an alternative but equivalentform of the product of exponentials formula. Note that M−1 is the relativedisplacement of the fixed frame as seen from the end-effector frame; since Bi =AdM−1(Si), assuming the chain is in its zero position, Bi admits a physicalinterpretation as the screw parameter for joint i, expressed in terms of the end-effector frame at the zero position.

Example: A 6R Spatial Elbow-Type Open Chain

Let us express the forward kinematics for the same 6R elbow-type open chainof Figure 4.8 in the alternative form

T06 = Me[B1]θ1e[B2]θ2 · · · e[B6]θ6 .

We assume the same fixed and end-effector frames and zero position as theprevious example. M is still the same as Equation (4.10), obtained as the end-effector frame as seen from the fixed frame with the chain in its zero position.


The screw vectors for each joint axes, however, are now expressed with respectto the end-effector frame in its zero position:

i ωi vi

1 ( 0, 0, 1) (−3L, 0, 0)2 ( 0, 1, 0) (0, 0, 0)3 (-1, 0, 0) (0, 0, −3L)4 (-1, 0, 0) (0, 0, −2L)5 (-1, 0, 0) (0, 0, −L)6 ( 0, 1, 0) (0, 0, 0)

4.3 Summary

• Given an open chain with a reference frame attached to some point on itslast link—this frame is denoted the end-effector frame—the forward kine-matics is the mapping from the joint values to the position and orientationof the end-effector frame.

• In the Denavit-Hartenberg representation, the forward kinematics of anopen chain is described in terms of relative displacements between ref-erence frames attached to each link. If the link frames are sequentiallylabelled {0}, . . . , {n}, where {0} is the fixed frame and {n} is the end-effector frame, then the forward kinematics is expressed as

T0n = T01(θ1) · · ·Tn−1,n(θn)

where θi denotes the joint i variable.

• The Denavit-Hartenberg convention requires that reference frames as-signed to each link obey a strict convention. Following this convention,the link frame transformation Ti−1,i between link frames {i− 1} and {i}can be parametrized using a minimum of four parameters (the Denavit-Hartenberg parameters): the link twist αi−1, link length ai−1, link offsetdi, and joint angle φi.

• The forward kinematics can also be expressed in the following product ofexponentials form:

T0n = e[S1]θ1 · · · e[Sn]θnM,

where Si = (ωi, vi) denotes the screw vector associated with joint i ex-pressed in fixed frame coordinates, θi is the joint i variable, and M ∈SE(3) denotes the position and orientation of the end-effector frame whenthe robot is in its zero position. A choice of fixed frame and end-effectorframe, together with a specification of the robot’s zero position and direc-tion of positive rotation or translation of each of the robot’s joints, thencompletely specifies the product of exponentials formula. In particular,no link frames are necessary.

4.3. Summary 101

• The product of exponentials formula can also be written in the equivalentform

T0n = Me[B1]θ1 · · · e[Bn]θn ,

where [B]i = M−1[Si]M , i = 1, . . . , n. Bi = (ωi, vi) is the screw vectorcorresponding to joint axis i, expressed in terms of the end-effector framewith the robot in its zero position.


In the original paper by Denavit and Hartenberg [7], the four parameters aredefined without appealing to any reference frames, e.g., the link length is definedto be length of the mutually perpendicular line between the joint axes, thetwist angle is obtained by first projecting the two adjacent axes to the planeperpendicular to the common normal and measuring the angle between the axes,etc. The convention of choosing the z axis to be along the joint axis and the xaxis to be along the common normal is not standard; other authors, for example,choose the joint axis to be in the x direction. The product of exponentialsformula is first presented by Brockett in [4]. Computational aspects of theproduct of exponentials formula are also discussed in [27].


4.4 Exercises

1. (a) For each given T ∈ SE(3)(3) below, find, if it exists, (α, a, d, φ) thatsatisfies

T = Rot(x, α)Trans(x, a)Trans(z, d)Rot(z, φ).

(a) T =

0 1 1 31 0 0 00 1 0 10 0 0 1

.

(b) T =

cosβ sinβ 0 1sinβ − cosβ 0 0

0 0 −1 −20 0 0 1

.

(c) T =

0 −1 0 −10 0 −1 01 0 0 20 0 0 1

.

2. Let T1, T2 ∈ SE(3), and suppose T1 = e[A1], T2 = e[A2] for some A1 =(ω1, v1), A2 = (ω2, v2). Under what conditions onA1 andA2 does T1T2 = T2T1?

Figure 4.10: A RRRP robot for performing pick-and-place operations.

3. The RRRP spatial open chain of Figure 4.10 is shown in its zero position.(a) Choose appropriate link reference frames, and derive the corresponding

4.4. Exercises 103

Denvait-Hartenberg parameters (use the given fixed frame.)(b) Find T04.

Figure 4.11: A RRPPRR open chain robot.

4. The RRPPRR spatial open chain of Figure 4.11 is shown in its zero position.Using the fixed reference frame given in the figure, choose appropriate linkreference frames and derive the Denavit-Hartenberg parameters.

5. The robot with a screw joint in Figure 4.12 is shown in its zero position.Using the given fixed frame, choose appropriate link reference frames and derivethe Denavit-Hartenberg parameters.

6. The spatial RPRRR open chain of Figure 4.13 is shown in its zero position.Using the given fixed reference frame, choose appropriate link reference framesand derive the Denavit-Hartenberg parameters.

7. The RRPRRR spatial open chain of Figure 4.14 is shown in its zero position(all joints lie on the same plane).(a) Using the given fixed frame, choose appropriate link reference frames andderive the Denavit-Hartenberg parameters.(b) Set θ5 = π and all the other joint variables to zero, and find T60.

8. The PRRRRR spatial open chain of Figure 4.15 is shown in its homeposition. Using the given fixed and end-effector frames, and the direction ofpositive rotation or translation for each joint, derive its forward kinematics inthe following product of exponentials form:

e[S1]θ1e[S2]θ2 · · · e[S6]θ6M


Figure 4.12: A robot with a screw joint.

Figure 4.13: An RPRRR spatial open chain.

4.4. Exercises 105

Figure 4.14: An RRPRRR spatial open chain.

Figure 4.15: A PRRRRR spatial open chain.

9. For the same open chain of Figure 4.15, express the forward kinematics inthe following product of exponentials form:

Me[B∞]θ1e[B2]θ2 · · · e[B6]θ6 .

10. The spatial RRPPRR open chain of Figure 4.16 is shown in its zeroposition.(a) Derive its forward kinematics in the form e[S1]θ1e[S2]θ2 · · · e[S6]θ6M .(b) Derive its forward kinematics in the form Me[B∞]θ1e[B2]θ2 · · · e[B6]θ6 .

11. The spatial RRRPRR open chain of Figure 4.17 is shown in its zero posi-tion.(a) Choose appropriate link frames and derive the corresponding Denavit-Hartenbergparameters.(b) Express its forward kinematics in the form e[S1]θ1e[S2]θ2 · · · e[S6]θ6M .


Figure 4.16: A spatial RRPPRR open chain with prescribed fixed and end-effector frames.

Figure 4.17: A spatial RRPRRR open chain with prescribed fixed and end-effector frames.

12. The spatial RRRRPR open chain of Figure 4.18 is shown in its zero posi-tion, with fixed and end-effector frames chosen as shown.(a) Choose appropriate link frames and derive the corresponding Denavit-Hartenbergparameters.(b) Express its forward kinematics in the form e[S1]θ1e[S2]θ2 · · · e[S6]θ6M .

4.4. Exercises 107

Figure 4.18: A spatial RRRRPR open chain.

Chapter 5

Differential Kinematics ofOpen Chains

A robot’s differential kinematics refers to the relationship between the robot’sjoint rates and the linear and angular velocity of its end-effector. Whereas theforward kinematics of typical robots is usually nonlinear and complicated, itturns out that the equations for differential kinematics are linear: at any in-stant during a robot’s motion, the end-effector’s linear and angular velocity canbe obtained simply by multiplying the joint rate vector by a (configuration-dependent) Jacobian matrix. This linear relationship is exploited to greateffect in many applications, ranging from algorithms for inverse kinematics andtrajectory generation to manipulation planning and control. The Jacobian ma-trix also turns out to play a central role in tasks involving static and dynamiccontact between the end-effector and the environment.

We begin this chapter with a review of particle kinematics, with a focuson using moving frames to calculate linear velocities and accelerations. Theconcept of angular velocity is also introduced in this moving frame context. Wethen derive the Jacobian matrix for open chains, and examine its role in velocityanalysis, statics, and the identification of kinematic singularities. Later chapterson inverse kinematics, motion planning, and control will also draw upon theseconcepts in a fundamental way.

5.1 Particle Kinematics

Referring to Figure 5.1, first choose a pair of fixed and moving frames for physicalspace, denoted {s} (for “space”) and {b} (for “body”—the moving frame istypicaly attached to a moving body), respectively. Denote the unit axes of thefixed frame by {X, Y , Z}, and those of the moving frame by {x, y, z}. As thename implies, the moving frame is assumed to move in physical space (imagine,for example, that the frame is attached to a robot end-effector)—this impliesthat not only will the origin of the moving frame change with time, but the

109

110 Differential Kinematics of Open Chains

Figure 5.1: Particle kinematics using moving reference frames.

Figure 5.2: (a) The instantaneous angular velocity vector. (b) Calculating ˙x.

directions of the axes {x, y, z} will as well.Within this setup, consider a particle ~p moving in physical space as shown

in the figure. At times we may find it convenient to represent the position of ~pin terms of both the fixed and moving frame coordinates:

~p(t) = X(t)X + Y (t)Y + Z(t)Z + x(t)x(t) + y(t)y(t) + z(t)z(t). (5.1)

The particle’s velocity, ~v(t), is obtained straightforwardly by differentating ~p(t)with respect to t:

~v(t) = XX + Y Y + ZZ + xx+ yy + zz + x ˙x+ y ˙y + z ˙z. (5.2)

Since {X, Y , Z} are fixed, their time derivatives are always zero; however,{ ˙x, ˙y, ˙z} clearly are not.

Let us now determine ˙x (and, by analogy, ˙y and ˙z). First, by definition thelength of x is always one; only the direction of x can vary with time (the samegoes for y and z).

Referring to Figure 5.2-(a), if we examine the moving frame at times t andt + ∆t—since we’ve argued above that what’s relevant is the moving frame’s

5.1. Particle Kinematics 111

orientation, for better visualization we draw the frame at the two instants withcoinciding origins—it should be clear that the change in frame orientation from tto t+∆t can be described as a rotation about some unit axis w passing throughthe origin, by some angle ∆θ. While it is not necessary to do so here, notethat we can obtain this ω and θ directly from the logarithm formula on SO(3):denoting the orientations at t and t + ∆t by R(t) and R(t + ∆t), respectively,it follows that R(t+ ∆t) = e[ω]θR(t), or [ω]θ = logR(t+ ∆t)RT (t).

In the limit as ∆t approaches zero, the ratio ∆θ∆t becomes the instantaneous

rate of rotation θ, and ω can similarly be regarded as the instantaneous axisof rotation. In fact, ω and θ can be put together to define the instantaneousangular velocity vector ~ω as follows:

~ω = ωθ. (5.3)

The term “instantaneous” here is a reminder of the fact that the rate of rotationθ and the rotation axis direction ω can both vary with time.

This time referring to Figure 5.2-(b), some basic trigonometry, combinedwith the rule for the cross-product of two vectors, should be enough to convinceoneself that

˙x = ~ω × x, (5.4)

and similarly,

˙y = ~ω × y (5.5)˙z = ~ω × z. (5.6)

Substituting these results into Equation (5.2) for the velocity ~v(t), we obtain,after some rearranging,

~v(t) = XX + Y Y + ZZ + xx+ yy + zz+~ω × (xx+ yy + zz) .

(5.7)

The acceleration of the moving particle can be obtained by differentiating ~v(t)with respect to t with the same rules applied to evaluate the time derivativesof x, y, and z. We now illustrate the above procedure for calculating velocitiesand accelerations with an example.

Example: A Telescopic Robot Arm

The telescopic robot arm of Figure 5.3 is an RRP open chain, in which thefirst two revolute joints are connected orthogonally, and the prismatic joint isconnected orthogonally to the second revolute joint. Suppose expressions forthe velocity and acceleration of the arm tip are desired. We first consider thecase where the the length s of the arm is assumed constant, and that the rates ofrotation ω1 and ω2 are also constant (the constant s assumption will be droppedlater).

Under these assumptions, first establish a fixed reference frame {X, Y , Z}such that its origin coincides with the intersection of joint axes 1 and 2—as


Figure 5.3: A telescopic robot arm.

witnessed earlier in the choice of link frames for the Denavit-Hartenberg rep-resentation, a judicious choice of moving frame can simplify the analysis con-siderably. For this problem we choose the moving frame depicted in the figure,which moves in such a way that x is always aligned along the ω2 axis of rotation,and y is always aligned along the length of the arm (there are, of course, otherchoices of moving frame that lead to the same result—we examine some of theseother possibilities in the exercises).

In terms of this moving frame, the vector ~p from the fixed frame origin tothe tip can then be written

~p = sy. (5.8)

Note that our expression for ~p is, by virtue of how our moving frame is defined,always valid at every configuration of the arm; otherwise the time derivative of~p will not lead to a correct expression for the velocity of ~p. The velocity is

~v = sy + s ˙y, (5.9)

where s = 0 (since s is assumed constant for the time being). Recalling that˙y = ~ω × y, we need to determine the instantaneous angular velocity vector ~ωof the moving frame. What is clear is that the moving frame is undergoingtwo rotations at the same time: about the always vertical joint axis 1 at aconstant rate ω1 (corresponding to the angular velocity vector ω1Z) and aboutthe horizontally aligned joint axis 2 at a constant rate ω2 (corresponding to theangular velocity vector ω2x).

We now assert that the angular velocity of the moving frame is the sum of

5.2. Angular Velocities 113

these two angular velocities:

~ω = ω1Z + ω2x. (5.10)

At an intuitive level, this assertion can be justified by observing the motionof the frame over an infinitesimal time interval ∆t: in this case the frame willhave undergone an infinitesimal rotation ∆θ1 about joint axis 1, followed by aninfitesimal rotation ∆θ2 about joint axis 2. Even though the direction of jointaxis 2 will have moved an infinitesimal amount as a result of the rotation ∆θ1

about axis 1, in the limit as ∆t approaches zero it can be argued that this effectbecomes negligible, and the resulting moving frame angular velocity is simplythe sum of the two component velocities. At a more mathematical level thisexplanation may be somewhat unsatisfying. This shall be remedied in the nextsection, where a complete and rigorous justification of this assertion is given.

Continuing, we have˙y = (ω1Z + ω2x)× y. (5.11)

Differentiating ~v leads to the acceleration ~a:

~a = s¨y= s(~ω × y + ~ω × ˙y)= ~ω × sy + ~ω × (~ω × sy),

(5.12)

where~ω = ω2

˙x= ω2((ω1Z + ω2x)× x)= ω1ω2(Z × x).

(5.13)

Let us now drop the assumption that the arm length is constant, and allows and s to be arbitrary. As before ~p = sy; differentiating ~p leads to

~v = sy + s ˙y = sy + ~ω × sy, (5.14)

where ~ω = ω1Z + ω2x. Differentiating ~v leads to the acceleration a:

~a = sy + s ˙y + s ˙y + s¨y= sy + 2s(ω × y) + ω × sy + ω × (ω × sy),

(5.15)

where we have used the fact that ˙y = ω × y + ω × ˙y. �

5.2 Angular Velocities

Consider a moving frame with axes {x, y, z} that rotates with angular velocity~ω. In the previous section we showed that

˙x = ~ω × x˙y = ~ω × y˙z = ~ω × z.


Let us now express these relations explicitly in terms of fixed frame coordinates.Let R(t) be the rotation matrix describing the orientation of the moving framewith respect to the fixed frame. Then the first column of R(t), denoted r1(t),describes x in fixed frame coordinates; similarly, r2(t) and r3(t) respectivelydescribe y and z in fixed frame coordinates. Let ωs ∈ R3 be the angular velocity~ω expressed in fixed frame coordinates (we use the subscript ‘s’ to denote the“space”, or fixed frame). The previous three velocity relations, expressed infixed frame coordinates, become

ri = ωs × ri = [ωs]ri, i = 1, 2, 3,

where [ωs], recall, is the 3× 3 skew-symmetric matrix representation of ωs.The above three equations can be rearranged into the following single matrix

equation:R = [ωs]R. (5.16)

Observe that since R−1 = RT , the above can also be written

RRT = [ωs]. (5.17)

This rather remarkable result shows that not only is the quantity RRT alwaysskew-symmetric, but also admits the physical interpretation as the angular ve-locity in fixed-frame coordinates.

Now let ωb be ~ω expressed in moving frame coordinates (here the subscript‘b’ denotes the moving, or “body” frame). To see how ωb can be obtained fromωs, and vice versa, let us momentarily return to our earlier notational practiceof writing R as Rsb: recall that Rsb represents the orientation of the movingframe {b} as seem from the fixed frame {s}. Since ωs and ωb are simply twodifferent coordinate representations of the same (free) vector ~ω, we can writeωs = Rsbωb, or ωb = R−1

sb ωs. Since R−1sb = RT , we have

ωb = RTωs. (5.18)

Let us now express the above relation in skew-symmetric matrix form. Recallfrom Chapter 3 that for any ω ∈ R3 and R ∈ SO(3), R[ω]RT = [Rω] alwaysholds. With this result in hand, we now apply the square bracket transformationto both sides of Equation (5.18):

[ωb] = [RTωs]= RT [ωs]R= RT (RRT )R= RT R

(5.19)

In summary, we have the following two fundamental equations that relate therotation matrix R to the angular velocity ~ω:

Proposition 5.1. Let R(t) denote the orientation of the moving frame as seenfrom the fixed frame. Denote by ~ω the instantaneous angular velocity of the

5.3. Spatial Velocities 115

moving frame. Then

RR−1 = [ωs] (5.20)R−1R = [ωb], (5.21)

where ωs ∈ R3 is the fixed frame representation of ~ω, and ωb ∈ R3 is the movingframe representation of ~ω.

5.3 Spatial Velocities

We now simultaneously consider the linear and angular velocity of the movingframe. As before, denote by {s} and {b} the fixed (space) and moving (body)frames, respectively, and let

Tsb(t) = T (t) =[R(t) p(t)

0 1

](5.22)

denote the homogeneous transformation of {b} as seen from {s} (to keep thenotation uncluttered, for the time being we shall write T instead of the usualTsb).

In the previous section we discovered that pre- or post-multiplying R byR−1 results in (a skew-symmetric representation of) the angular velocity vector,either in fixed or moving frame coordinates. One might reasonably ask if asimilar property carries over to T ; that is, do T−1T and T T−1 carry similarphysical interpretations?

Let us first see what happens when we pre-multiply T by T−1:

T−1T =[RT −RT p0 1

] [R p0 0

]=

[RT R RT p

0 0

]=

[[ωb] vb0 0

].

(5.23)

Recall that RT R = [ωb] is just the (skew-symmetric matrix representation of)the angular velocity, expressed in moving frame coordinates. Also, p is thelinear velocity of the moving frame origin expressed in fixed frame coordinates,and RT p = vb is this same velocity expressed in moving frame coordinates.Putting these two observations together, we can conclude that T−1T representsthe linear and angular velocity of the moving frame in terms of the movingframe coordinates.

Just from an examination of the physical units of ωb and vb, on the onehand it would seem perfectly natural to think of linear and angular velocities asbeing distinct quantities, and to treat them separately. On the other hand, theprevious calculation of T−1T suggests some justification for merging ωb and vbinto a single six-dimensional quantity. This is in fact what we shall do (and this


Figure 5.4: Physical interpretation of vs. The initial (solid line) and displaced(dotted line) configurations of a rigid body.

is one of the defining features of the modern screw-theoretic approach to robotkinematics). Define

Vb =[ωbvb

], [Vb] =

[[ωb] vb0 0

]= T−1T . (5.24)

At times we shall also use the slightly more compact notation Vb = (ωb, vb).[Vb] is then the 4× 4 matrix representation of Vb. We shall call Vb the spatialvelocity in the moving (or body) frame.

The above derivation of T−1T would seem to offer some hope that, assumingthings behave symmetrically, T T−1 will turn out to be the linear and angularvelocity vectors in the fixed frame. Let us see if this is indeed the case. Post-multiplying T by T−1 leads to

T T−1 =[R p0 0

] [RT −RT p0 1

]=

[RRT p− RRT p

0 0

]=

[[ωs] vs0 0

].

(5.25)

Recall that the skew-symmetric matrix [ωs] = RRT is the angular velocityexpressed in fixed frame coordinates; this is the good news. Unfortunately, thequantity vs = p−RRT p is not the hoped-for linear velocity of the moving frameorigin expressed in the fixed frame (that quantity is simply p). On the otherhand, if we write vs as

vs = p− ωs × p = p+ ωs × (−p), (5.26)

5.3. Spatial Velocities 117

the physical meaning of vs now becomes clear: imagining the moving frame isattached to an infinitely large rigid body, vs is the instantaneous velocity of thepoint on this body corresponding to the fixed frame origin (see Figure 5.4).

While it may seem unfortunate that vs cannot be interpreted, similar toωs, as simply the linear velocity of the moving frame origin expressed in thefixed frame, if we regard the fixed (ground) and moving bodies equally as beinginfinitely large, there is in fact an intuitively appealing and natural symmetrybetween (ωs, vs) and (ωb, vb):

• ωb is the angular velocity in moving frame coordinates;

• ωs is the angular velocity in fixed frame coordinates;

• vb is the linear velocity of the moving frame origin, in moving framecoordinates;

• vs is the linear velocity of the fixed frame origin (regarded as a pointon the moving rigid body), in fixed frame coordinates.

In the same way that we defined Vb, let us define

Vs =[ωsvs

], [Vs] =

[[ωs] vs0 0

]= T T−1, (5.27)

with Vs sometimes denoted more compactly as Vs = (ωs, vs). The symmetricrelationship between Vs and Vb offers some justification in referring to Vs asthe fixed (or space) frame representation of the spatial velocity. It is asimple matter to obtain Vb from Vs, and vice versa:

[Vb] = T−1T

= T−1(T T−1)T= T−1 [Vs]T,

(5.28)

and[Vs] = T [Vb]T−1. (5.29)

The reader may recognize that the relation between Vs and Vb is given pre-cisely by the transformation rule for a screw vector under a change of referenceframes. In fact, using the adjoint mapping AdT the above can be expressed asVs = AdT (Vb), Vb = AdT−1(Vs). This transformation rule can be more easilyremembered if we write T = Tsb, in which case

Vs = AdTsb(Vb) (5.30)Vb = AdTbs(Vs). (5.31)

In expanded form, the above becomes[ωsvs

]=

[R 0

[p]R R

] [ωbvb

](5.32)[

ωbvb

]=

[RT 0

−RT [p] RT

] [ωsvs

]. (5.33)


The main results on spatial velocities derived thus far are summarized in thefollowing proposition:

Proposition 5.2. Given a fixed (space) frame {s} and moving (body) frame{b}, let Tsb(t) ∈ SE(3) be differentiable, where

Tsb(t) =[R(t) p(t)

0 1

]. (5.34)

Then

T−1sb Tsb = [Vb] =

[[ωb] vb0 0

](5.35)

is the spatial velocity in body coordinates, and

TsbT−1sb = [Vs] =

[[ωs] vs0 0

](5.36)

is the spatial velocity in space coordinates. Vs and Vb are related by

Vs =[ωsvs

]=[

R 0[p]R R

] [ωbvb

]= AdTsb(Vb) (5.37)

Vb =[ωbvb

]=[

RT 0−RT [p] RT

] [ωsvs

]= AdTbs(Vs). (5.38)

5.4 Manipulator Jacobian

5.4.1 Space Jacobian

This section derives the relationship between an open chain’s join rates (θ1, . . . , θn)and and its end-effector’s spatial velocity Vs. We first recall a few facts fromChapter 3 concerning linear differential equations: (i) if A,B ∈ Rn×n are bothinvertible, then (AB)−1 = B−1A−1; (ii) if A ∈ Rn×n is constant and θ(t) ascalar function of t, then d

dteAθ = AeAθ θ = eAθAθ; (iii) (eAθ)−1 = e−Aθ.

Now consider an n-link open chain whose forward kinematics is expressed inthe following product of exponentials form:

T (θ1, . . . , θn) = e[S1]θ1e[S2]θ2 · · · e[Sn]θnM. (5.39)

The spatial velocity of the end-effector frame with respect to the fixed frame,Vs, is given by [Vs] = T T−1, where

T = (d

dte[S1]θ1) · · · e[Sn]θnM + e[S1]θ1(

d

dte[S2]θ2) · · · e[Sn]θnM + . . .

= [S1]θ1e[S1]θ1 · · · e[Sn]θnM + e[S1]θ1 [S2]θ2e

[S2]θ2 · · · e[Sn]θnM + . . .

Also,T−1 = M−1e−[Sn]θn · · · e−[S1]θ1 .

5.4. Manipulator Jacobian 119

Multiplying T and T−1, we have

[Vs] = [S1]θ1 + e[S1]θ1 [S2]e−[S1]θ1 θ2 + e[S1]θ1e[S2]θ2 [S3]e−[S2]θ2e−[S1]θ1 θ3 + . . . .

The above can also be expressed in vector form by means of the adjoint mapping:

Vs = S1θ1 + Ade[S1]θ1 (S2)θ2 + Ade[S1]θ1e[S2]θ2 (S3)θ3 + . . . (5.40)

Observe that Vs is a sum of n spatial velocities of the form

Vs = Vs1(θ)θ1 + . . .+ Vsn(θ)θn, (5.41)

where each Vsi(θ) = (ωsi(θ), vsi(θ)) depends explictly on the joint values θ ∈ Rn.In matrix form,

Vs =[Vs1(θ) Vs2(θ) · · · Vsn(θ)

] θ1

...θn

= Js(θ)θ.

(5.42)

The matrix Js(θ) is said to be the Jacobian in fixed (space) frame coordinates,or more simply the space Jacobian.

Definition 5.1. Let the forward kinematics of an n-link open chain be expressedin the following product of exponentials form:

T = e[S1]θ1 · · · e[Sn]θnM. (5.43)

The space Jacobian Js(θ) ∈ R6×n relates the joint rate vector θ ∈ Rn to theend-effector spatial velocity Vs via Vs = Js(θ)θ. The i-th column of Js(θ) is

Vsi(θ) = Ade[S1]θ1 ···e[Si−1]θi−1 (Si), (5.44)

for i = 2, . . . , n, with the first column Vs1(θ) = S1. �

To understand the physical meaning behind the columns of Js(θ), recallfrom Chapter 3 that if Sa = (ωa, va) is the vector describing a screw motion inframe {a} coordinates, and Sb = (ωb, vb) is a vector describing the same screwmotion in frame {b} coordinates, then Sb and Sa are related by Sb = AdTba(Sa)(see Figure 5.5-(a)). Another physical interpretation of this transformation isfrom the perspective of reference frame {a} only. Referring to Figure 5.5-(b),suppose the vector Sa describes the initial screw axis with respect to frame {a},and the vector Sb describes the screw axis after it has undergone a rigid bodydisplacement Tba. It follows that

ωb = Rbaωa. (5.45)

The point q on the screw axis is now displaced from its initial location qa to

qb = Tbaqa = Rbaqa + pba. (5.46)


Figure 5.5: Physical interpretation of the screw adjoint transformation AdTba :(a) describing the same screw motion in terms of two different reference frames{a} and {b}; (b) An initial screw axis displaced a transformation Tba.

Then from the definition vb = −ωb × qb + hbωb, where hb = ha (the screw pitchis a scalar quantity, and hence independent of choice of reference frames), wehave

vb = −ωb × qb + hbωb= −Rbaωa × (Rbaqa + pba) + hbRbaωa= Rba (−[ωa]qa + haωa)−Rba[ωa]RTbapba= Rbava + [pba]Rbaωa,

where in the last two lines we have again made use of the matrix identityR[ω]RT = [Rω] for R ∈ SO(3) and ω ∈ R3. Equations (5.45) and (5.47)can be combined in the form[

ωbvb

]=[

Rba 0[pba]Rba Rba

] [ωava

], (5.47)

which is precisely Sb = AdTba(Sa).Returning now to the equation for the space Jacobian (5.40), observe that

the i-th term of the right-hand side of (5.40) is of the form AdTi−1(Si), whereTi−1 = e[S1]θ1 · · · e[Si−1]θi−1 ; recall here that Si is the screw vector describingthe i-th joint axis in terms of the fixed frame with the robot in its zero position.AdTi−1(Si) can therefore be viewed as the screw vector describing the i-th jointaxis after it undergoes the rigid body displacement Ti−1. But physically this isthe same as moving the first i− 1 joints from their zero position to the currentvalues θ1, . . . , θi−1. Therefore, the i-th column Vsi(θ) of Js(θ) is simply thescrew vector describing joint axis i, expressed in fixed frame coordinates as afunction of the joint variables θ1, . . . , θi−1.

In summary, the procedure for determining the columns of Js(θ) is similar tothat for deriving the Si in the product of exponentials formula e[S1]θ1 · · · e[Sn]θnM :each column Vsi(θ) is the screw vector describing joint axis i, expressed in fixedframe coordinates, but for arbitrary θ rather than θ = 0.


s

L1

1

23

4L2

L3

T

Figure 5.6: Space Jacobian for a spatial RRRP chain.

Example: Space Jacobian for a Spatial RRRP Chain

We now illustrate the procedure for finding the space Jacobian for the spatialRRRP chain of Figure 5.6. Denote the i-th column of Js(θ) by Vi = (ωi, vi).

• Observe that ω1 is constant and in the z-direction: ω1 = (0, 0, 1). Pickingq1 to be the origin, v1 = (0, 0, 0).

• ω2 is also constant in the z-direction, so ω2 = (0, 0, 1). Pick q2 = (L1c1, L1s1, 0),where c1 = cos θ1, s1 = sin θ1. Then v2 = −ω2 × q2 = (L1s1,−L1c1, 0).

• The direction of ω3 is always fixed in the z-direction regardless of thevalues of θ1 and θ2, so ω3 = (0, 0, 1). Picking q3 = (L1c1 + L2c12, L1s1 +L2s12, 0), where c12 = cos(θ1 + θ2), s12 = sin(θ1 + θ2), it follows thatv3 = (L1s1 + L2s12,−L1c1 − L2c12, 0).

• Since the final joint is prismatic, ω4 = (0, 0, 0), and the joint axis directionis given by v4 = (0, 0,−1).

The space Jacobian is therefore

Js(θ) =

0 0 0 00 0 0 01 1 1 00 L1s1 L1s1 + L2s12 00 −L1c1 −L1c1 − L2c12 00 0 0 −1

.


s

q1

qw

L1

L2

12

3

4

5

6

Figure 5.7: Space Jacobian for the spatial RRPRRR chain.

Example: Space Jacobian for Spatial RRPRRR Chain

We now derive the space Jacobian for the spatial RRPRRR chain of Figure 5.7.The base frame is chosen as shown in the figure.

• The first joint axis is in the direction ω1 = (0, 0, 1). Picking q1 = (0, 0, L1),we get v1 = −ω1 × q1 = (0, 0, 0).

• The second joint axis is in the direction ω2 = (−c1,−s1, 0). Picking q2 =(0, 0, L1), we get v2 = −ω2 × q2 = (L1s1,−L1c1, 0).

• The third joint is prismatic, so ω3 = (0, 0, 0). The direction of the pris-matic joint axis is given by

v3 = Rot(z, θ1)Rot(x,−θ2)

010

=

−s1c2c1c2−s2

.• Now consider the wrist portion of the chain. The wrist center is located

at the point

qw =

00L1

+Rot(z, θ1)Rot(x,−θ2)

0L1 + θ3

0

=

−(L2 + θ3)s1c2(L2 + θ3)c1c2

L1 − (L2 + θ3)s2

.Observe that the directions of the wrist axes depend on θ1, θ2, and the


preceding wrist axes. These are

ω4 = Rot(z, θ1)Rot(x,−θ2)

001

=

−s1s2

c1s2

c2

ω5 = Rot(z, θ1)Rot(x,−θ2)Rot(z, θ4)

−100

=

−c1c4 + s1c2s4

−s1c4 − c1c2s4

s2s4

ω6 = Rot(z, θ1)Rot(x,−θ2)Rot(z, θ4)Rot(x,−θ5)

010

=

−c5(s1c2c4 + c1s4) + s1s2s5

c5(c1c2c4 − s1s4)− c1s2s5

−s2c4c5 − c2s5

.The space Jacobian can now be computed and written in matrix form as follows:

Js(θ) =[ω1 ω2 0 ω4 ω5 ω6

0 −ω2 × q2 v3 −ω4 × qw −ω5 × qw −ω6 × qw

].

Although the resulting Jacobian is quite complicated, note that we were ableto calculate the entire Jacobian without having to explicitly differentiate theforward kinematic map.

5.4.2 Body Jacobian

In the previous section we derived the relationship between the joint rates and[Vs] = T T−1, the end-effector’s spatial velocity expressed in fixed frame coordi-nates. Here we derive the relationship between the joint rates and [Vb] = T−1T ,the end-effector spatial velocity in end-effector frame coordinates. For this pur-pose it will be more convenient to express the forward kinematics in the alternateproduct of exponentials form:

T (q) = Me[B1]θ1e[B2]θ2 · · · e[Bn]θn . (5.48)

Computing T ,

T = Me[B1]θ1 · · · e[Bn−1]θn−1(d

dte[Bn]θn) +Me[B1]θ1 · · · ( d

dte[Bn−1]θn−1)e[Bn]θn + . . .

= Me[B1]θ1 · · · e[Bn]θn [Bn]θn +Me[B1]θ1 · · · e[Bn−1]θn−1 [Bn−1]e[Bn]θn θn−1 + . . .

+Me[B1]θ1 [B1]e[B2]θ2 · · · e[Bn]θn θ1.

Also,T−1 = e−[Bn]θn · · · e−[B1]θ1M−1.

Multiplying T−1 and T ,

[Vb] = [Bn]θn + e−[Bn]θn [Bn−1]e[Bn]θn θn−1 + . . .

+e−[Bn]θn · · · e−[B2]θ2 [B1]e[B2]θ2 · · · e[Bn]θn θ1,


or in vector form,

Vb = Bnθn + Ade−[Bn]θn (Bn−1)θn−1 + . . .+ Ade−[Bn]θn ···e−[B2]θ2 (B1)θ1. (5.49)

Vb can therefore be expressed as a sum of n spatial velocities, i.e.,

Vb = Vb1(θ)θ1 + . . .+ Vbn(θ)θn, (5.50)

where each Vbi(θ) = (ωbi(θ), vbi(θ)) depends explictly on the joint values θ. Inmatrix form,

Vb =[Vb1(θ) Vb2(θ) · · · Vbn(θ)

] θ1

...θn

= Jb(θ)θ.

(5.51)

The matrix Jb(q) is the Jacobian in the end-effector (or body) frame coordi-nates, or more simply the body Jacobian.

Definition 5.2. Let the forward kinematics of an n-link open chain be expressedin the following product of exponentials form:

T = Me[B1]θ1 · · · e[Bn]θn . (5.52)

The body Jacobian Jb(θ) ∈ R6×n relates the joint rate vector θ ∈ Rn to theend-effector spatial velocity Vb = (ωb, vb) via

Vb = Jb(θ)θ. (5.53)

The i-th column of Jb(θ) is given by

Vb,i(θ) = Ade−[Bn]θn ···e−[Bi+1]θi+1 (Bi), (5.54)

for i = n, n− 1, . . . , 2, with Vbn(θ) = Bn. �

Analogous to the columns of the space Jacobian, a similar physical inter-pretation can also be given to the columns of Jb(θ): each column Vbi(θ) =(ωbi(θ), vbi(θ)) of Jb(θ) is the screw vector for joint axis i, expressed in coordi-nates of the end-effector frame rather than the fixed frame. The procedure fordetermining the columns of Jb(θ) is similar to the procedure for deriving theforward kinematics in the product of exponentials form Me[B1]θ1 · · · e[Bn]θn , theonly difference being that each of the joint screws are derived for arbitrary θrather than θ = 0.

5.4.3 Relationship between the Space and Body Jacobian

If we denote the fixed frame by {s}, and the robot arm’s end-effector frame by{b}, then the forward kinematics can be written Tsb(θ). The spatial velocity

5.5. Statics of Open Chains 125

of the tip frame can be written in terms of the fixed and end-effector framecoordinates as

[Vs] = TsbT−1sb (5.55)

[Vb] = T−1sb Tsb, (5.56)

with Vs and Vb related by Vs = AdTsb(Vb). Vs and Vb are also related to theirrespective Jacobians via

Vs = Js(θ)θ (5.57)Vb = Jb(θ)θ. (5.58)

Equation (5.57) can therefore be written

AdTsb(Vb) = Js(θ)θ. (5.59)

Applying AdTbs to both sides of (5.59), and using the general property AdM ·AdN = AdMN of the adjoint map, we obtain

AdTbs(AdTsb(Vb)) = AdTbsTsb(Vb)= Vb= AdTbs

(Js(q)θ

).

Since we also have Vb = Jb(θ)θ for all θ, it follows that Js(θ) and Jb(θ) arerelated by

Js(θ) = AdTsb (Jb(θ)) . (5.60)

Writing AdTsb in 6× 6 matrix form [AdTsb ], the above can also be expressed as

Js(θ) = [AdTsb ]Jb(θ). (5.61)

The body Jacobian can in turn be obtained from the space Jacobian via

Jb(θ) = AdTbs (Js(θ)) = [AdTbs ]Js(θ). (5.62)

5.5 Statics of Open Chains

A rigid body is said to be in static equilibrium if it is motionless, and theresultant forces and moments applied to the body are all zero. Let us brieflyreview the notion of moments, by considering a force ~f acting on a rigid body.If the rigid body’s center of mass does not lie on the line of action of the force,then the force will cause the rigid body to rotate; this rotation is caused by amoment. More precisely, the moment ~m generated by ~f about some referencepoint p in physical space is defined to be the cross product

~m = ~r × ~f, (5.63)


where ~r is the vector from p to the point on the rigid body at which the force isapplied. For a rigid body subject to a collection of forces and moments, if boththe sum of the forces and sum of the moments are zero, then the body will bestationary, and is said to be in static equilibrium. When summing moments itis important that each of the moments be expressed with respect to the samereference point p.

A robot arm is said to be in static equilibrium if all of its links are in staticequilibrium. In this section we shall examine, for robot arms that are in staticequilibrium, the relationship between any external forces and moments appliedat the end-effector, and the forces and torques experienced at each of the joints.Such a situation arises, for example, when a six degree-of-freedom arm is pushingagainst an immobile wall.

We first review spatial forces (also referred to as wrenches in the classicalscrew theory literature), which are obtained by merging forces and momentsinto a single six-dimensional quantity, much like spatial velocities are obtainedby merging linear and angular velocities into a single six-dimensional vector. Aswe did for spatial velocities, we shall also examine how spatial forces transformunder a change of reference frames.

We then review the principle of virtual work, but expressed in terms of spatialvelocities and spatial forces. Applying the virtual work principle to a robot armassumed to be in static equilibrium leads to our main result, which states thatany external spatial forces applied at the end-effector frame are linearly relatedto the torques experienced at the joints.

5.5.1 Spatial Forces

Just as we found it advantageous to merge a moving frame’s angular velocityω ∈ R3 with its linear velocity v ∈ R3 into a single six-dimensional spatialvelocity V = (ω, v), for the same reasons it will be useful to analogously definea six-dimensional spatial force, by merging a three-dimensional force vectorf ∈ R3 with a three-dimensional moment vector m ∈ R3 as follows:

F =[mf

], (5.64)

which for notational convenience we will also write F = (m, f).Let us find explicit expressions for the spatial force in terms of specific ref-

erence frames. For this purpose consider a rigid body with a moving (body)frame {b} attached. Expressing everything in terms of the {b} frame, let fb ∈ R3

denote a force vector that is applied to a point p on the body. This force thengenerates a moment with respect to the {b} frame origin; in {b} frame coordi-nates, this moment is

mb = rb × fb, (5.65)

where rb ∈ R3 is the vector from the {b} frame origin to p. We shall pair theforce fb and moment mb into a single six-dimensional spatial force Fb = (mb, fb),and refer to it as the spatial force in body frame coordinates.


Figure 5.8: Relation between Fb and Fs.

Suppose we now wish to express the force and moment in terms of the fixed(space) frame {s}. Let fs ∈ R3 denote the force vector being applied to pointp of the rigid body, this time expressed in {s} frame coordinates. The momentgenerated by this force with respect to the {s} frame origin is, again in {s}frame coordinates,

ms = rs × fs, (5.66)

where rs ∈ R3 is the vector from the {s} frame origin to p. As we did for Fb, letus also bundle fs and ms into the six-dimensional spatial force Fs = (ms, fs),and refer to it as the spatial force in space frame coordinates.

We now determine the relation between Fb = (mb, fb) and Fs = (ms, fs).Referring to Figure 5.8, denote the transformation Tsb by

Tsb =[Rsb psb0 1

].

Pretty clearly fb = Rbsfs, which with the benefit of hindsight we shall write inthe somewhat unconventional form

fb = RTsbfs. (5.67)

The moment mb is given by rb × fb, where rb = Rbs(rs − psb); this follows fromthe fact that the rs − psb is expressed in {s} frame coordinates, and must betransformed to {b} frame coordinates via multiplication by Rbs. Again withhindsight, we shall write

rb = RTsb(rs − psb).

The moment mb = rb × fb can now be written in terms of fs and ms as

mb = RTsb(rs − psb)×RTsbfs= [RTsbrs]R

Tsbfs − [RTsbpsb]R

Tsbfs

= RTsb[rs]fs −RTsb[psb]fs= RTsbms +RTsb[psb]

T fs,

(5.68)


where in the last line we make use of the fact that [psb]T = −[psb]. Writing bothmb and fb in terms of ms and fs, we have, from Equations (5.67) and (5.68),[

mb

fb

]=[

Rsb 0[psb]Rsb Rsb

]T [ms

fs

], (5.69)

or in terms of spatial forces and the adjoint map,

Fb = AdTTsb(Fs) = [AdTsb ]TFs. (5.70)

We see that under a change of reference frames, spatial velocities transform un-der the adjoint map, whereas spatial forces transform under the adjoint trans-pose map. In fact, the introduction of the rigid body is superfluous; the aboverelation holds for all spatial forces described in terms of two different referenceframes. The following proposition formally states this result.

Proposition 5.3. Given a force ~f , let ~m be the moment generated by ~f withrespect to some point p in physical space. Given a reference frame {a}, letfa ∈ R3 and ma = ra × fa ∈ R3 be representations of ~f and ~m in frame {a}coordinates, where ra ∈ R3 is the vector from the {a} frame origin to p, alsoexpressed in {a} frame coordinates. Similarly, given another reference frame{b}, let fb ∈ R3 and mb = rb × fb ∈ R3 be representations of ~f and ~m frame{b} coordinates, where rb ∈ R3 is the vector from the {b} frame origin to p, alsoexpressed in {b} frame coordinates. Defining the spatial forces Fa = (ra×fa, fa)and Fb = (rb × fb, fb), Fa and Fb are related by

Fb = AdTTab(Fa) = [AdTab ]TFa (5.71)

Fa = AdTTba(Fb) = [AdTba ]TFb. (5.72)

5.5.2 Static Analysis and the Virtual Work Principle

When a robot arm is in static equilibrium, it turns out that the Jacobian of theforward kinematics also relates any external forces and torques applied at theend-effector to the torques experienced at each of the joints. This can be shownby appealing to the Principle of Virtual Work, which we now describe. Fixa reference frame, and consider a rigid body moving with velocity v and angularvelocity ω, and subject to a resultant force f and resultant moment m. Thework done by the rigid body over some time interval [t0, t1] is given by theintegral

Work =∫ t1

t0

fT v +mTω dt (5.73)

In terms of spatial forces and velocities, (5.73) can also be expressed as

Work =∫ t1

t0

FTV dt, (5.74)


where F = (m, f) and V = (ω, v). The work of a system of rigid bodies issimply the sum of the work done by each of the rigid bodies.

For a single rigid body, suppose that the resultant force and moment areapplied to the body over an infinitesimal time interval δt, resulting in an in-finitesimally small displacement of the body. If the body is in static equilibrium,the body is stationary and thus will produce no work. This infinitesimal dis-placement over δt can still be thought of as a virtual displacement. The virtualwork principle states that under static equilibrium, the work of any externalforces and moments acting on a rigid body is always zero for any admissiblevirtual displacement of the body. This principle also extends to robot arms,and more generally to any system of connected rigid bodies: for any admissiblevirtual displacement of the system (i.e., one that does not violate any kinematicconstraints), the total virtual work of the external forces and moments actingon the system is zero.

Now consider an n-link robot arm assumed to be in static equilibrium, andsuppose a force and moment are applied to the tip. For now all quantitiesare defined in terms of the end-effector (body) frame: let Fb = (mb, fb) be anexternal spatial force applied over some infinitesimal time interval [t0, t0 + δt],and Vb = (ωb, vb) be the (instantaneous) spatial velocity of the end-effector. Thenet virtual work done by the robot is given by Equation (5.73). Assuming therobot is lossless, from the virtual work principle this infinitesimal work shouldbe the same as that produced by any torques applied at the joints:

Virtual Work =∫ t0+δt

t0

FTb Vb dt =∫ t0+δt

t0

τT θ dt,

where θ ∈ Rn is the vector of joint velocities, and τ ∈ Rn is the vector of jointtorques. Since Vb = Jb(θ)θ, we have∫ t0+δt

t0

FTb Jb(θ)θ dt =∫ t0+δt

t0

τT θ dt.

Since this equality must hold over all intervals [t0, t0 + δt], the integrands mustbe equal:

FTb Jb(θ)θ = τT θ.

Moreover, since the above equality must hold for all admissible virtual displace-ments θ δt—in this case θ can be arbitrary—it follows that

τ = JTb (θ)Fb. (5.75)

Let us replicate the derivation of τ = JTb (θ)Fb, but this time expressingall quantities in terms of the fixed (space) frame. Let Vs = (ωs, vs) denote thespatial velocity of the end-effector, and Fs = (ms, fs) the spatial force applied atthe end-effector frame origin, all expressed in fixed frame coordinates. Here fs isthe external applied force expressed in fixed frame coordinates, while ms is theexternal applied moment about the fixed frame origin. Recalling from the earlier


section on spatial forces that Fb and Fs are related by Fb = [AdTsb]TFs, and

that Jb(θ) and Js(θ) are further related by Jb(θ) = [AdTbs ]Js(θ), Equation (5.75)can be rewritten

τ = JTb (θ)Fb = ([AdTbs ]Js(θ))T [AdTsb]

TFs= JTs (θ) ([AdTsb][AdTbs ])

T Fs= JTs (θ)Fs.

(5.76)

We can therefore write the statics relation in the general form

τ = JT (θ)F , (5.77)

with the understanding that J(θ) and F are expressed in terms of the sameframe. Often in robotics one is interested in determining what joint torques arenecessary, under static equilibrium assumptions, to produce a given desired F ;the static relation provides an explicit answer to this question.

One could also ask the opposite question, namely, what is the spatial forceat the tip generated by a given joint torque? If JT is a square invertible matrix,then clearly F = J−T (θ)τ . However, if the dimension of the joint vector n isgreater than the dimension of F (six), then the inverse of JT does not exist.What this implies physically is that the robot arm has extra degrees of freedom.Because of the extra degrees of freedom, some of the robot’s links can moveeven when the end-effector is fixed (for example, the planar four-bar linkagecan be regarded as a 3R planar open chain with its tip fixed to the base joint).Internal motions are generated as a result of the applied joint torques, and thestatic equilibrium condition is no longer satisfied. Robots whose joint degreesof freedom exceed the dimension of its task space are called kinematicallyredundant; in the next chapter we shall examine the inverse kinematics ofkinematically redundant robot arms.

5.6 Singularities

The forward kinematics Jacobian also allows us to identify postures at whichthe robot’s end-effector loses the ability to move instantaneously in a certaindirection, or rotate instantaneously about certain axes; such a posture is calleda kinematic singularity, or simply a singularity. Mathematically a singularposture of a robot arm is one in which the Jacobian loses rank. To understandwhy, consider the body Jacobian Jb(θ), whose columns are denoted Vbi, i =1, . . . , n. Then

Vb =[Vb1(θ) Vb2(θ) · · · Vbn(θ)

] θ1

...θn

= Vb1(θ)θ1 + . . .+ Vbn(θ)θn.

Thus, the set of all possible instantaneous spatial velocities of the tip frame isgiven by a linear combination of the Vbi. As long as n ≥ 6, the maximum rank

5.6. Singularities 131

Figure 5.9: Kinematic singularities are invariant with respect to choice of fixedand end-effector frames. (a) Choosing a different fixed frame, which is equivalentto relocating the base of the robot arm; (b) Choosing a different end-effectorframe.

that Jb(θ) can attain is six. Singular postures correspond to those values ofθ at which the rank of Jb(θ) drops below six; at such postures the tip frameloses to the ability to generate instantaneous spatial velocities in in one or moredimensions.

The mathematical definition of a kinematic singularity is independent of thechoice of body or space Jacobian. To see why, recall the relationship betweenJs(θ) and Jb(θ): Js(q) = [AdTsb (Jb(θ)) = [AdTsb ]Jb(θ), or more explicitly,

Js(θ) =[

Rsb 0[psb]Rsb Rsb

]Jb(θ).

Then the rank of Js(θ) is equal to the rank of [AdTsb ]Jb(θ). We now claim thatthe matrix [AdTsb ] is always invertible. This can be established by examiningthe linear equation [

Rsb 0[psb]Rsb Rsb

] [xy

]= 0.

Its unique solution is x = y = 0, implying that the matrix [AdTsb ] is invertible.Since multiplying any matrix by an invertible matrix does not change its rank,it follows that

rank Js(θ) = rank Jb(θ),

as claimed; singularities of the space and body Jacobian are the one and thesame.

Kinematic singularities are also independent of the choice of fixed frame. Insome sense this is rather obvious—choosing a different fixed frame is equivalentto simply relocating the robot arm, which should have absolutely no effect onwhether a particular posture is singular or not. This obvious fact can be verifiedby referring to Figure 5.9-(a). The forward kinematics with respect to the


Figure 5.10: A kinematic singularity in which two joint axes are collinear.

original fixed frame is denoted T (θ), while the forward kinematics with respectto the relocated fixed frame is denoted T ′(θ) = PT (θ), where P ∈ SE(3) isconstant. Then the body Jacobian of T ′(θ), denoted J ′b(θ), is obtained fromT ′−1T ′. A simple calculation reveals that

T ′−1T ′ = (T−1P−1)(PT ) = T−1T ,

i.e., J ′b(θ) = Jb(θ), so that the singularities of the original and relocated robotarms are the same.

Somewhat less obvious is the fact that kinematic singularities are also inde-pendent of the choice of end-effector frame. Referring to Figure 5.9-(b), supposethe forward kinematics for the original end-effector frame is given by T (θ), whilethe forward kinematics for the relocated end-effector frame is T ′(θ) = T (θ)Q,where Q ∈ SE(3) is constant. This time looking at the space Jacobian—recallthat singularities of Jb(θ) coincide with those of Js(θ)—let J ′s(θ) denote thespace Jacobian of T ′(θ). A simple calculation reveals that

T ′T ′−1 = (TQ)(Q−1T−1) = T T−1,

i.e., J ′s(θ) = Js(θ), so that kinematic singularities are invariant with respect tochoice of end-effector frame.

In the remainder of this section we shall consider some common kinematicsingularities that occur in six degree of freedom open chains with revolute andprismatic joints. We now know that either the space or body Jacobian can beused for our analysis; we shall use the space Jacobian in the examples below.

Case I: Two Collinear Revolute Joint Axes

The first case we consider is one in which two revolute joint axes are collinear(see Figure 5.10). Without loss of generality these joint axes can be labelled 1


x

yz

q1q3q2

Figure 5.11: A kinematic singularity in which three revolute joint axes areparallel and coplanar.

and 2. The corresponding columns of the Jacobian are

Vs1(θ) =[

ω1

−ω1 × q1

], Vs2(θ) =

[ω2

−ω2 × q2

]Since the two joint axes are collinear, we must have ω1 = ±ω2; let us assumethe positive sign. Also, ωi × (q1 − q2) = 0 for i = 1, 2. Then Vs1 − Vs2 = 0,which implies that Vs1 and Vs2 lie on the same line in six-dimensional space.Therefore, the set {Vs1,Vs2, . . . ,Vs6} cannot be linearly independent, and therank of Js(θ) must be less than six.

Case II: Three Coplanar and Parallel Revolute Joint Axes

The second case we consider is one in which three revolute joint axes are parallel,and also lie on the same plane (three coplanar axes—see Figure 5.11). Withoutloss of generality we label these as joint axes 1, 2, and 3. In this case we choosethe fixed frame as shown in the figure; then

Js(θ) =[ω1 ω1 ω1 · · ·0 −ω1 × q2 −ω1 × q3 · · ·

]and since q2 and q3 are points on the same unit axis, it is not difficult to verifythat the above three vectors cannot be linearly independent.

Case III: Four Revolute Joint Axes Intersecting at a Common Point

Here we consider the case where four revolute joint axes intersect at a commonpoint (Figure 5.12). Again, without loss of generality label these axes from 1to 4. In this case we choose the fixed frame origin to be the common point ofintersection, so that q1 = . . . = q4 = 0. In this case

Js(θ) =[ω1 ω2 ω3 ω4 · · ·0 0 0 0 · · ·

].


A1

A2

A3

A4

Figure 5.12: A kinematic singularity in which four revolute joint axes intersectat a common point.

The first four columns clearly cannot be linearly independent; one can be writ-ten as a linear combination of the other three. Such a singularity occurs, forexample, when the wrist center of an elbow-type robot arm is directly abovethe shoulder.

Case IV: Four Coplanar Revolute Joints

Here we consider the case in which four revolute joint axes are coplanar. Again,without loss of generality label these axes from 1 to 4. Choose a fixed framesuch that the joint axes all lie on the x-y plane; in this case the unit vectorωi ∈ R3 in the direction of joint axis i is of the form

ωi =

ωixωiy0

.Similarly, any reference point qi ∈ R3 lying on joint axis i is of the form

qi =

qixqiy0

,and subsequently

vi = −ωi × qi =

00

ωiyqix − ωixqiy

.

5.7. Manipulability 135

Figure 5.13: The manipulability ellipsoid for a planar 2R open chain.

The first four columns of the space Jacobian Js(θ) areω1x ω2x ω3x ω4x

ω1y ω2y ω3y ω4y

0 0 0 00 0 0 00 0 0 0

ω1yq1x − ω1xq1y ω2yq2x − ω2xq2y ω3yq3x − ω3xq3y ω4yq4x − ω4xq4y

.

which clearly cannot be linearly independent.

Case V: Six Revolute Joints Intersecting a Common Line

The final case we consider is six revolute joint axes intersecting a common line.Choose a fixed frame such that the common line lies along the z-axis, and selectthe intersection between this common line and joint axis i as the reference pointqi ∈ R3 for axis i; each qi is thus of the form qi = (0, 0, qiz), and

vi = −ωi × qi = (ωiyqiz,−ωixqiz, 0)

i = 1, . . . , 6. The space Jacobian Js(θ) thus becomes

Js(θ) =

ω1x ω2x ω3x ω4x ω5x ω6x

ω1y ω2y ω3y ω4y ω5y ω6y

ω1z ω2z ω3z ω4z ω5z ω6z

ω1yq1z ω2yq2z ω3yq3z ω4yq4z ω5yq5z ω6yq6z

−ω1xq1z −ω2xq2z −ω3xq3z −ω4xq4z −ω5xq5z −ω6xq6z

0 0 0 0 0 0

,

which is clearly singular.

5.7 Manipulability

In the previous section we saw that at a kinematic singularity, a robot’s end-effector loses the ability to move or rotate in one or more directions. A kinematic


singularity is a binary proposition—a particular configuration is either kinemat-ically singular, or it isn’t—and it is reasonable to ask whether it is possible toquantify the proximity of a particular configuration to a singularity. The answeris yes; in fact, one can even do better and quantify not only the proximity to asingularity, but also determine the directions in which the end-effector’s abilityto move is diminished, and to what extent. The manipulabiity ellipsoid al-lows one to geometrically visualize the directions in which the end-effector canmove with the least “effort” (in a sense to be made precise below); directionsthat are orthogonal to these directions in contrast require the greatest effort.

We illustrate the notion of manipulability ellipsoids through the planar 2Ropen chain example of Figure 5.13. Considering only the Cartesian position ofthe tip, the differential forward kinematics is of the form x = J(θ), θ:[

x1

x2

]=[

L1 cos θ1 L1 cos θ1 + L2 cos(θ1 + θ2) L1 sin θ1 L1 sin θ1 + L2 sin(θ1 + θ2)

] [θ1

θ2

]. (5.78)

Suppose the configuration θ is nonsingular, so that J(θ) is invertible. Since J(θ)is a linear transformation that maps joint velocities to tip velocities, one canconjecture that a unit circle in the space of joint velocities maps to an ellipsoidin the space of tip velocities. To see why, the unit circle is parametrized by theconstraint ‖θ‖2 = 1; the same constraint expressed in terms of tip velocities is‖J−1(θ)x‖2 = 1. Denoting the elements of J−1(θ) by

J−1(θ) =[a bc d

],

the constraint on tip velocities becomes

αx21 + βx1x2 + γx2

2 = 1,

where α = a2 + b2, β = 2(ab + cd), γ = b2 + d2. As is well known this is theequation for an ellipse centered at the origin (provided β2 − 4αγ < 0, which itis).

The major axes of the ellipse indicate the directions in which the tip canmove (or more technically, generate velocities) with the least amount of effort(effort here corresponding to input velocities). By the same reasoning, the minoraxes indicate the directions of motion for which the greatest amount of effortis required. As the arm configuration approaches a kinematic singularity, theellipsoid eventually collapses to a line directed along the direction of allowablemotion. The proximity of a particular configuration to a singularity can be mea-sured in several ways, e.g., by the ratio of the lengths between the major andminor axes, with a minimum value of 1 indicating that the tip can move uni-formly easily in all directions—such configurations are also sometimes referredto as isotropic configurations. In the absence of any preferred directions of theend-effector, such an isotropic configuration would be a reasonable choice for arobot performing generic tasks.

5.8. Summary 137

The above formulation can be generalized to spatial open chains more or lessstraightforwardly; without working out the details of the derivation (which re-quire some further results from linear algebra and finite dimensional optimization—some of these details are examined in the exercises), we now formulate themanipulability ellipsoid for an n degree of freedom open chain (n ≥ 6). LetJb(θ) ∈ R6×n be the body Jacobian (the choice of body Jacobian is arbitrary—the space Jacobian could just as easily have been chosen), which can be par-titioned into its angular and linear velocity components Jω(θ) ∈ R3×n andJv(θ) ∈ R3×n as follows:

ωb = Jω(θ)θ (5.79)vb = Jv(θ)θ. (5.80)

At this point one may ask why we choose to partition the Jacobian in this way.The reason is that the notion of a manipulability ellipsoid in the six-dimensioalspace of spatial velocities (ω, v) makes little if any sense—the physical units forangular velocities are different from those for linear velocities. Any ellipsoidthat merges these physically different quantities will depend, ultimately, on thechoice of length scale for physical space, which as is well known is arbitrary. Onthe other hand, ellipsoids restricted to the space of Cartesian velocities vb ∈ R3

are quite meaningful (as are its counterpart ellipsoids restricted to the space ofangular velocities ω ∈ R3).

We now formulate the Cartesian velocity manipulability ellipsoid associatedwith Jv(θ); the angular velocity manipulability ellipsoid can be formulated inan identical fashion. Assuming Jv(θ) is nonsingular at the configuration θ,Jv(θ) then maps a unit sphere in Rn, parametrized as ‖θ‖2 = 1, to a three-dimensional ellipsoid in R3. The principal axes of this ellipsoid can be obtainedas the eigenvectors of JvJTv ∈ R3×3, with the length of each principal axis givenby the corresponding eigenvalue.

A three-dimensional ellipsoid can also be drawn in the space of joint velocitiesas follows. First, observe that JTv Jv is n×n, but with rank 3 (as a result of ourassumption that Jv(θ) is of maximal rank 3). Consequently only three of itseigenvalues are nonzero; they are, in fact, the three eigenvalues of JvJTv . Thethree eigenvectors corresponding to these nonzero eigenvalues are then preciselythe joint velocity vectors that map to the three principal axes of the ellipsoid inthe space of Cartesian velocities.

In the absence of any preferred directions of the end-effector, one can arguethat an ellipsoid that most closely resembles a sphere is the most desirable.Configurations in which the ellipsoid is spherical are called isotropic configura-tions, and are marked by the eigenvalues—which are proportional to the lengthsof the ellipsoid’s principal axes—having identical value.

5.8 Summary

• Suppose a frame with unit axes {x, y, z} is moving in physical space withlinear velocity ~v and angular velocity ~ω. The velocities of its unit axes are


then respectively given by

˙x = ~ω × x˙y = ~ω × y˙z = ~ω × z.

If the position ~p of a moving particle is expressed in terms of both thefixed frame {X, Y , Z} and moving frame {x, y, z}, i.e.,

~p(t) = X(t)X + Y (t)Y + Z(t)Z + x(t)x+ y(t)y + z(t)z,

its velocity can be obtained by differentiating ~p(t), making use of theprevious formulas for ˙x, ˙y, and ˙z.

• Let R(t) ∈ SO(3) describes the orientation of a moving (or body) referenceframe as seen from the fixed (or space) frame, and ~ω be the instantaneousangular velocity of the moving frame. let ωs ∈ R3 be ~ω expressed in thespace frame, and ωb ∈ R3 be ~ω expressed in the body frame. ωs and ωbcan be obtained from R(t) via

[ωs] = RRT

[ωb] = RT R.

where [ωs] and [ωb] denote the 3×3 skew-symmetric matrix representationsof ωs and ωb, respectively. ωs and ωb are related by

ωs = Rsbωb = Rωb

ωb = Rbsωs = RTωs.

• Given a space (fixed) frame {s} attached to ground, and a body (moving)frame {b} attached to a rigid body moving in physical space, let

Tsb(t) = T (t) =[R(t) p(t)

0 1

]∈ SE(3)

denote the spatial displacement of {b} as seen from {s}. Then

T T−1 =[RRT p− RRT p

0 0

]=[

[ωs] vs0 0

]= [Vs]

T−1T =[RT R RT p

0 0

]=[

[ωb] vb0 0

]= [Vb].

Vs = (ωs, vs) ∈ R6 is called the spatial velocity in space frame co-ordinates, while Vb = (ωb, vb) ∈ R6 is the spatial velocity in bodyframe coordinates. Here ωs ∈ R3 and ωb ∈ R3 respectively denote theangular velocity of the body frame expressed in space and body framecoordinates. vb ∈ R3 denotes the linear velocity of the body frame origin,

5.8. Summary 139

expressed in body frame coordinates. vs ∈ R3 denotes the linear velocityof the space frame origin (regarded as a point on the moving rigid body),expressed in space frame coordinates. Vs and Vb are related by

[Vs] = T [Vb]T−1

[Vb] = T−1[Vs]T,

or, alternatively,

Vs =[ωsvs

]=[

R 0[p]R R

] [ωbvb

]= AdTsb(Vb)

Vb =[ωbvb

]=[

RT 0−RT [p] RT

] [ωsvs

]= AdTbs(Vs).

AdT is called the adjoint mapping with respect to T ∈ SE(3). Thematrix [AdT ] ∈ R6×6 is defined to be

[AdT ] =[

R 0[p]R R

].

• Let the forward kinematics of an n-link open chain be expressed in thefollowing product of exponentials form:

T = e[S1]θ1 · · · e[Sn]θnM.

The space Jacobian Js(θ) ∈ R6×n relates the joint rate vector θ ∈ Rnto the end-effector spatial velocity Vs via Vs = Js(θ)θ. The i-th columnof Js(θ) is

Vsi(θ) = Ade[S1]θ1 ···e[Si−1]θi−1 (Si), (5.81)

for i = 2, . . . , n, with the first column Vs1(θ) = S1. Vsi is the screw vectorfor joint i expressed in space frame coordinates, with the joint values θassumed to be arbitrary rather than zero.

• Let the forward kinematics of an n-link open chain be expressed in thefollowing product of exponentials form:

T = Me[B1]θ1 · · · e[Bn]θn . (5.82)

The body Jacobian Jb(θ) ∈ R6×n relates the joint rate vector θ ∈ Rn tothe end-effector spatial velocity Vb = (ωb, vb) via

Vb = Jb(θ)θ. (5.83)

The i-th column of Jb(θ) is given by

Vb,i(θ) = Ade−[Bn]θn ···e−[Bi+1]θi+1 (Bi), (5.84)

for i = n, n− 1, . . . , 2, with Vbn(θ) = Bn. Vbi is the screw vector for jointi expressed in body frame coordinates, with the joint values θ assumed to


be arbitrary rather than zero. The body Jacobian is related to the spaceJacobian via the relation

Js(θ) = [AdT sb]Jb(θ)Jb(θ) = [AdT bs]Js(θ)

where Tsb = T .

• Consider a force ~f applied to some point p on a rigid body. The moment~m generated by ~f with respect to the {s} frame origin is ~m = ~r× ~f , where~r is the vector from p to the {s} frame origin. Let ms ∈ R3 and fs ∈ R3 bevector representations of ~m and ~f in {s} frame coordinates. The spatialforce in space coordinates Fs ∈ R6 is defined to be Fs = (ms, fs).

• Consider a rigid body with a body frame {b} attached, and a force ~fapplied to some point p on the rigid body. The moment ~m generated by~f with respect to the {b} frame origin is then ~m = ~r × ~f , where ~r is nowthe vector from p to the {b} frame origin. Let mb ∈ R3 and fb ∈ R3 bevector representations of ~m and ~f in {b} frame coordinates. The spatialforce in body coordinates Fb ∈ R6 is defined to be Fb = (mb, fb). Fband Fs are related by

Fb = AdTTsb(Fs) = [AdTsb ]TFs

Fs = AdTTbs(Fb) = [AdTbs ]TFb.

• Consider a spatial open chain with n one degree of freedom joints that isalso assumed to be in static equilibrium. Let τ ∈ Rn denote the vector ofjoint torques and forces, and F ∈ R6 be the spatial force applied at theend-effector, in either space or body frame coordinates. Then τ and F arerelated by

τ = JTb (θ)Fb = JTs (θ)Fs.

• A kinematically singular configuration for an open chain, or more simplya kinematic singularity, is any configuration θ ∈ Rn at which the rankof the Jacobian (either Js(θ) or Jb(θ)) is not maximal. For spatial openchains of mobility six consisting of revolute and prismatic joints, somecommon singularities include (i) two collinear revolute joint axes; (ii) threecoplanar and parallel revolute joint axes; (iii) four revolute joint axesintersecting at a common point; (iv) four coplanar revolute joints, and (v)six revolute joints intersecting a common line.


The terms spatial velocity and spatial force were first coined by Roy Feath-erstone [10], and are also referred to in the literature as twists and wrenches,respectively. There is a well developed calculus of twists and wrenches that is

5.9. Notes and References 141

covered in treatments of classical screw theory, e.g., [3], [2]. Singularities ofclosed chains are discussed in the later chapter on closed chain kinematics. Ma-nipulability ellipsoids and their dual, force ellipsoids, are discussed in greaterdetail in [30].


Figure 5.14: Polar coordinates.

5.10 Exercises

1. Given a particle moving in the plane, define a moving reference frame {er, eθ}such that its origin is fixed to the origin of the fixed frame, and its er axis alwayspoints toward the particle (Figure 5.14). Let (r, θ) be the polar coordinates forthe particle position, i.e., r is the distance of the particle from the origin, and θis the angle from the horizontal line to the er axis. The particle position ~p canthen be written

~p = rer,

and its velocity ~v is given by

~v = rer + r ˙er.

The acceleration ~v is the time derivative of ~v.(a) Express ˙er in terms of er and eθ.(b) Show that ~v and ~a are given by

~v = rer + rθeθ

~a = (r − rθ2)er + (rθ + 2rθ)eθ.

2. Let {I , J} denote the unit axes of the fixed frame, and let

~p = X(t)I + Y (t)J

denote the position of a particle moving in the plane (see Figure 5.15). Supposethe path traced by the particle has nonzero curvature everywhere, so that forevery point on the path there exists some circle tangent to the path; the centerof this circle is called the center of curvature, while its radius is the radiusof curvature. Clearly both the center and radius of curvature vary along thepath, and are well-defined only at points of nonzero curvature (or, at pointswhere the curvature is zero, the center of curvature can be regarded to lie atinfinity).

5.10. Exercises 143

Figure 5.15: Tangential-normal coordinates.

Now attach a moving reference frame {et, en} to the particle, in such a waythat that et always points in the same direction as the velocity vector; en thenpoints toward the center of curvature. Since the speed of the particle is givenby

v =√X2 + Y 2.

and et always points in the direction of the velocity vector, it follows that thevelocity vector ~v of the particle can be written

~v = vet,

while its acceleration is given by

~a = vet + v ˙et.

(a) Show that ˙et = ‖ ˙et‖en, or

en =˙et‖ ˙et‖

,

and that consequently the acceleration ~a is

~a = vet + v‖ ˙et‖en.

(b) The radius of curvature ρ can be found from the following formula:

ρ =v3

XY − Y X

=(X2 + Y 2)3/2

XY − Y X.


Figure 5.16: A cannon mounted on a 2R rotating platform.

Using the formula, show that the acceleration ~a can be written

~a = vet +v2

ρen.

3. In standard treatments of particle kinematics using moving frames, twomoving particles, A and B, are assumed, with a moving frame {x, y, z} attachedto particle A. Writing the position of particle B as

~pB = ~pA + ~pB|A,

where ~pB|A denotes the vector from A to B, the following formulas for thevelocity and acceleration of B are usually provided:

~vB = ~vA +(~pB|A

)xyz

+ ~ω × ~pB|A

~aB = ~aA +(~pB|A

)xyz

+ 2~ω ×(~pB|A

)xyz

+ ~α× ~pB|A + ~ω × (~ω × ~pB|A),

where ~ω and ~α respectively denote the angular velocity and angular acceler-ation vector of the moving frame, and

(~pB|A

)xyz

(~pB|A

)xyz

are certain time

derivatives of ~pB|A. Writing ~pA and ~pB|A in terms of fixed and moving framecoordinates, i.e.,

~pA = XX + Y Y + ZZ

~pB|A = xx+ yy + zz,

derive the above formulas for ~vB and ~aB . Be sure to explicitly identify all terms,in particular

(~pB|A

)xyz

and(~pB|A

)xyz

.

5.10. Exercises 145

Figure 5.17: A circular plate of radius R.

4. Figure 5.16 depicts a cannon mounted on a 2R rotating platform at timet = 0. The platform rotates at constant angular velocities ω1 and ω2 radians/secas shown. The axis of the cannon barrel is displaced at a distance d from theorigin of the fixed frame. Assume that a cannonball is fired at t = 0 from thesame height as the I axis, at a constant speed v0 along the axis of the barrel.(a) Choose a moving frame and describe how the frame moves.(b) Determine the velocity of the cannonball at t = 0 in terms of the movingframe chosen in part (a).

5. As shown in Figure 5.17, a revolving circular plate of radius R, rotating ata constant angular velocity of ω2 radians/sec, is mounted on a wheeled mobilebase that moves periodically back and forth along the I axis according to

x(t) = a sinω1t.

(a) Assuming t = 0 at the instant shown in the figure, determine the velocityof point A as a function of t in fixed frame coordinates.(b) Determine the acceleration of point A as a function of t in fixed framecoordinates.

6. The circular pipe of Figure 5.18 is rotating about the X axis at a constantrate ω1 radians/sec, while a marble D is circling the pipe at a constant speedu.(a) Find the angle θ at which the magnitude of the velocity of D is maximal.What is the maximal velocity magnitude at this angle?(b) Find the angle θ at which the magnitude of the acceleration of D is maximal.What is the maximal acceleration magnitude at this angle?


Figure 5.18: A marble traversing a rotating circular pipe.

Figure 5.19: A satellite with a rotating panel.

7. The satellite of Figure 5.19 is rotating about its own vertical K axis at aconstant rate ω1 radians/sec, while at the same time its solar panel rotates ata constant rate ω2 radians/sec as shown.(a) Determine the velocity of point A when ω1 = 0.5, ω2 = 0.25, l = 2m,r = 0.5m, and θ = 30◦.(b) Determine the acceleration of point A under the same conditions as part(a).

8. The two revolute joints in the spherical 2R open chain of Figure 5.20 rotat-ing at constant angular velocities ω1 radians/sec and ω2 radians/sec as shown.Denote by r the length of link AB, while θ is the angle between link AB andthe x-y plane.(a) Choose a moving frame and explain how the frame moves.(b) Determine the angular velocity and angular of link AB in terms of yourmoving frame coordinates chosen in part (a).(c) Determine the velocity of point B in terms of the chosen moving frame co-ordinates.

5.10. Exercises 147

Figure 5.20: A spherical 2R open chain.

Figure 5.21: A rotating disk.

(d) Determine the acceleration of point B in terms of the chosen moving framecoordinates.(e) Setting ω1 = 0.1, ω2 = 0.2, and r = 100mm, determine the velocity andacceleration of point B in terms of the fixed frame coordinates when θ = π/6.

9. As shown in Figure 5.21, a disk of radius r spins at a constant angular


Figure 5.22: A toroidal 2R open chain.

velocity of ω2 radians/sec about its horizontal axis, while at the same time thedisk assembly rotates about the vertical axis at a constant angular velocity ofω1 radians/sec.(a) Determine the angular velocity and the angular acceleration of the disk interms of fixed frame coordinates.(b) Determine the velocity and the acceleration of point P as a function of theangle θ.

10. As shown in Figure 5.22, the two revolute joints of the toroidal 2R openchain are rotating at a constant angular velocity ω1 = 0.6 radians/sec about theY axis, and ω2 = 0.45 radians/sec about the horizontal axis through C. Whenβ = 120◦, determine the following in terms of fixed frame coordinates:(a) the angular acceleration of link CD.(b) the velocity of point D.(c) the acceleration of point D.

11. Figure 5.23 shows an RRP open chain at t = 0. The revolute joints rotateat constant angular velocities ω1 and ω2 radians/sec. Suppose the vertical po-sition of point B is given by x(t) = sin t. Determine the following quantities interms of fixed frame coordinates.(a) The velocity of point B at t = 0.(b) The acceleration of point B at t = 0.

12. The square plate of Figure 5.24 rotates about axis I with angular velocityω2 = 0.5 radians/sec and angular acceleration α2 = 0.01 radians/sec2, whilethe circular disk attached to the square plate rotates about the axis normal

5.10. Exercises 149

Figure 5.23: An RRP open chain.

Figure 5.24: A rotating square plate.

to the plate with angular velocity ω1 = 1 radians/sec and angular accelerationα1 = 0.5 radians/sec2. The radius of the circular disk is R = 5m, while thelength of each side of the square plate is 2R = 10m. The distance from thecenter of the circular disk to the small circular knob is d = 3m. Assume thatboth the disk and the square plate have zero thickness. Setting θ1 = 0◦ andθ2 = 45◦, find the following in terms of fixed frame coordinates:(a) The velocity of the circular knob.(b) The acceleration of the circular knob.

13. A person is riding the 2R gyro swing of Figure 5.25. Joint θ oscillatesperiodically according to θ(t) = cos t, and the circular plate connected to theaxis of the gyro swing rotates with constant angular velocity ω2 radians/sec. Att = 0, the person on the circular plate is at the maximal height as shown in thefigure. Setting l = 1, r = 1, and ω2 = 1 radian/sec, determine the velocity ofthe person in terms of the given fixed frame coordinates when t = π

2 .


Figure 5.25: A 2R gyro swing.

Figure 5.26: A meteorite approaching the earth.

14. As shown in Figure 5.26, a meteorite is approaching a rotating asteroidalong the meteorite’s J axis with velocity v1 = 100 m/sec. Suppose the radiusof the asteroid is R = 1000m, and the distance of the meteorite from the asteroidis initially D = 107m. The asteroid takes 6 hours to complete a full revolution.An astronaut stands at the point antipodal to the expected point of collision,and unwittingly starts walking north along a longitudinal arc at a velocity ofv2 = 1 m/sec. After three hours, determine the velocity of the astronaut in termsof the moving frame coordinates attached to the meteorite. of the moving frameat the meteorite.

15. As shown in Figure 5.27, a clock of radius r is mounted on a gimbalassembly as shown. The angles θ1, θ2, and θ3 are adjustable to arbitrary values;in the figure the angles are all assumed to be set to zero. A moving frame {T}is attached to the tip of the clock’s second hand, with its x-axis aligned alongthe tip of the hand as shown. Setting r = 1m , a = 3m, b = 7m, answer the

5.10. Exercises 151

Figure 5.27: A clock mounted on a gimbal assembly.

Figure 5.28: A 3R planar open chain.

following:(a) Assuming the second hand starts at 12 at t = 0, when the second handreaches 10, find TST ∈ SE(3) as a function of the angles (θ1, θ2, θ3).(b) Setting θ1 = 90◦, θ2 = 0, θ3 = 90◦, find the the velocity of the tip of thesecond hand at the moment it passes 10.

16. The 3R planar open chain of Figure 5.28 is shown in its zero position.(a) Suppose the tip must apply a force of 5N in the x-direction. What torquesshould be applied at each of the joints?


Figure 5.29: A planar 4R open chain.

(b) Suppose the tip must now apply a force of 5N in the y-direction. Whattorques should be applied at each of the joints?

17. Answer the following questions for the 4R planar open chain of Figure 5.29.(a) Derive the forward kinematics in the form

T (θ) = e[S1]θ1e[S2]θ2e[S3]θ3e[S4]θ4M.

where each S〉 ∈ R3 and M ∈ SE(2).(b) Derive the body Jacobian.(c) Suppose the chain is in static equilibrium at the configuration θ1 = θ2 =0, θ3 = π

2 , θ4 = −π2 , and a force f = (10, 10, 0) and moment m = (0, 0, 10) areapplied to the tip (both f and m are expressed with respect to the fixed frame).What are the torques experienced at each of the joints?(d) Under the same conditions as (c), suppose that a force f = (−10, 10, 0)and moment m = (0, 0,−10) are applied to the tip. What are the torquesexperienced at each of the joints?(e) Find all kinematic singularities for this chain.

18. Referring to Figure 5.30, the rigid body rotates about the point (L,L) withangular velocity θ = 1.(a) Find the position of point P on the moving body with respect to the fixedreference frame in terms of θ.(b) Find the velocity of point P in terms of the fixed frame.(c) What is Tfb, the displacement of frame {b} as seen from the fixed frame{f}?(d) Find the spatial velocity of Tfb in body coordinates.

5.10. Exercises 153

θ

Figure 5.30: A rigid body rotating in the plane.

x

y

z

{S}

L

x

y

z

{T}

L1

2

3 4

Figure 5.31: An RRRP spatial open chain.

(e) Find the spatial velocity of Tfb in space coordinates.(f) What is the relation between the spatial velocities obtained in (d) and (e)?(g) What is the relation between the spatial velocity obtained in (d) and Pobtained in (b)?(h) What is the relation between the spatial velocity obtained in (e) and Pobtained in (b)?

19. The RRRP chain of Figure 5.31 is shown in its zero position.(a) Determine the body Jacobian Jb(θ) when θ1 = θ2 = 0, θ3 = π/2, θ4 = L.(b) Determine the linear velocity of the end-effector frame, in fixed frame coor-dinates, when θ1 = θ2 = 0, θ3 = π/2, θ4 = L and θ1 = θ2 = θ3 = θ4 = 1.

20. The spatial 3R open chain of Figure 5.32 is shown in its zero position.(a) In its zero position, suppose we wish to make the end-effector move withlinear velocity vtip = (10, 0, 0), where vtip is expressed with respect to the spaceframe {s}. What are the necessary input joint velocities θ1, θ2, θ3?


θ1

xyz

{ S }

θ2

θ3

xyz

{ b}

2L

L

L L

Figure 5.32: A spatial 3R open chain.

(b) Suppose the robot is in the configuration θ1 = 0, θ2 = 45◦, θ3 = −45◦.Assuming static equilibrium, suppose we wish to generate an end-effector forcefb = (10, 0, 0), where fb is expressed with respect to the end-effector frame {b}.What are the necessary input joint torques τ1, τ2, τ3?(c) Under the same conditions as in (b), suppose we now seek to generate anend-effector moment mb = (10, 0, 0), where mb is expressed with respect to theend-effector frame {b}. What are the necessary input joint torques τ1, τ2, τ3?(d) Suppose the maximum allowable torques for each joint motor are

‖τ1‖ ≤ 10, ‖τ2‖ ≤ 20, ‖τ3‖ ≤ 5.

In the home position, what is the maximum force that can be applied by thetip in the end-effector frame x-direction?

21. The spatial PRRRRP open chain of Figure 5.33 is shown in its zeroposition.(a) At the zero position, find the first three columns of the space Jacobian.(b) Find all configurations at which the first three columns of the space Jacobianbecome linearly dependent.(c) Suppose the chain is in the configuration θ1 = θ2 = θ3 = θ5 = θ6 =0, θ4 = 90o. Assuming static equilibrium, suppose a pure force fb = (10, 0, 10)is applied to the origin of the end-effector frame, where fb is expressed in termsof the end-effector frame. Find the joint torques τ1, τ2, τ3 experienced at thefirst three joints.

22. Consider the PRRRRR spatial open chain of Figure 5.34 shown in its zeroposition. The distance from the origin of the fixed frame to the origin of theend-effector frame at the home position is L.(a) Determine the first three columns of the space Jacobian Js.(b) Determine the last two columns of the body Jacobian Jb.

5.10. Exercises 155

z y

x

Z

YX

θ θ

θ

θ

z

yx

θ

θ

Figure 5.33: A spatial PRRRRP open chain.

2

1 4

5

6

xy

z

xy

z{S} {T}3

L

100N

Figure 5.34: A PRRRRR spatial open chain.

(c) For what value of L is the home position a singularity?(d) In the zero position, what joint torques must be applied in order to generatea pure end-effector force of 100N in the -z direction?

23. Find all kinematic singularities of the 3R wrist with the following forwardkinematics:

R = e[ω1]θ1e[ω2]θ2e[ω3]θ3

where ω1 = (0, 0, 1), ω2 = (1/√

2, 0, 1/√

2), and ω3 = (1, 0, 0).

24. Show that a six degree of freedom spatial open chain is in a kinematicsingularity when any two of its revolute joint axes are parallel, and any prismatic


Prismatic joint axis

Revolute joint axis

Figure 5.35: A kinematic singularity involving prismatic and revolute joints.

L L L

x

y

z

{T}x

y

z

{S}

Figure 5.36: Singularities of an elbow-type 6R open chain.

joint axis is normal to the plane spanned by the two parallel revolute joint axes(see Figure 5.35).

25. (a) Determine the space Jacobian Js(θ) of the 6R spatial open chain ofFigure 5.36.(b) Find the kinematic singularities of the given chain. Explain each singularityin terms of the alignment of the joint screws, and the directions in which theend-effector loses one or more degrees of freedom of motion.

26. The spatial PRRRRP open chain of Figure 5.37 is shown in its zeroposition.(a) Determine the first 4 columns of the space Jacobian Js(θ).(b) Determine whether the zero position is a kinematic singularity.(c) Calculate the joint torques required for the tip to apply the following end-effector spatial forces:

(i) Fs = (0, 1,−1, 1, 0, 0)T

(ii) Fs = (1,−1, 0, 1, 0,−1)T .

5.10. Exercises 157

X

Y

Z

{S}

1

3

4

1

1 1

1

2

1

5 6

x

y

z

{T}

45o

Figure 5.37: A spatial PRRRRP open chain with a skewed joint axis.

L0

L1 L2

L1

θ1

θ2

θ3θ4

θ5

θ6

z

yx

z

yx

{ t }

{ 0’}xyz

{ 0 }

Figure 5.38: A spatial RRPRRR open chain.

27. The spatial RRPRRR open chain of Figure 5.38 is shown in its zeroposition.(a) For the fixed frame {0} and tool frame {t} as shown, express the forwardkinematics in the following product of exponentials form:

T (θ) = e[S1]θ1e[S2]θ2e[S3]θ3e[S4]θ4e[S5]θ5e[S6]θ6M.

(b) Find the first three columns of the space Jacobian Js(θ).(c) Suppose that the fixed frame {0} is moved to another location {0′} as shownin the figure. Find the first three columns of the space Jacobian Js(θ) withrespect to this new fixed frame.


θ

Figure 5.39: A rollercoaster undergoing a screw motion.

(d) Determine if the zero position is a kinematic singularity, and if so, providea geometric description in terms of the joint screw axes.

28. The rollercoaster of Figure 5.39 undergoes a screw motion as shown: pointA traces a circle of radius R, and the rollercoaster moves in screw-like fashion ata distance r from this larger circle. The roller coaster completes one revolutionabout this larger circle when point A traverses 30◦ along the larger circle.(a) Find T12, the relative displacement of the rollercoaster frame {2} as seenfrom the fixed frame {1}, in terms of the angle θ as indicated in the figure.(b) Derive the space Jacobian for T12(θ).

29. Two frames {a} and {b} are attached to a moving rigid body. Show thatthe spatial velocity of {a} in space frame coordinates is the same as the spatialvelocity of {b} in space frame coordinates.

30. Consider an n-link open chain, with reference frames attached to each link.Let

T0k = e[S1]θ1 · · · e[Sk]θkMk, k = 1, . . . , n

be the forward kinematics up to link frame {k}. Let Js(θ) be the space Jacobianfor T0n. The columns of Js(θ)are denoted

Js(θ) =[Vs1(θ) · · · Vsn(θ)

].

Let [Vk] = ˙T0kT−10k be the spatial velocity of link frame {k} in frame {k} coor-

dinates.(a) Derive explicit expressions for V2 and V3.(b) Based on your results from (a), derive a recursive formula for Vk+1 in termsof Vk, Vs1, . . . ,Vs,k+1, and θ.

Chapter 6

Inverse Kinematics of OpenChains

For a general n degree of freedom open chain with forward kinematics T (θ),θ ∈ Rn, the inverse kinematics problem can be stated as follows: given a ho-mogeneous transform X ∈ SE(3), find solutions θ that satisfy T (θ) = X. Tohighlight the main features of the inverse kinematics problem, let us considerthe two-link planar open chain of Figure 6.1-(a) as a motivational example.Considering only the position of the end-effector and ignoring its orientation,the forward kinematics can be expressed as[

xy

]=[L1 cos θ1 + L2 cos(θ1 + θ2)L1 sin θ1 + L2 sin(θ1 + θ2)

]. (6.1)

Assuming L1 > L2, the set of reachable points, or the workspace, is an annulusof innner radius L1 − L2 and outer radius L1 + L2. Given some end-effectorposition (x, y), it is not hard to see that there will be either zero, one, or twosolutions depending on whether (x, y) lies in the exterior, boundary, or interiorof this annulus (the case of two solutions is given by the familiar elbow-up andelbow-down configurations as illustrated in Figure ??).

Finding an explicit solution (θ1, θ2) for a given (x, y) is also not difficult. Re-ferring this time to Figure 6.1-(b), assume that (x, y) lies in the first quadrant,i.e., both x and y are positive (solutions for the other quadrants follow straight-forwardly). Angle β, restricted to lie in the interval [0, π], can be determinedfrom the law of cosines:

L21 + L2

2 − 2L1L2 cosβ = x2 + y2,

from which it follows that

β = cos−1

(L2

1 + L22 − x2 − y2

2L1L2

)θ2 = π − β.

159

160 Inverse Kinematics of Open Chains

(a) Workspace and elbow-up andelbow-down configurations.

(b) Geometric solution.

Figure 6.1: 2R planar open chain inverse kinematics.

Also from the law of cosines,

α = cos−1

(x2 + y2 + L2

1 − L22

2L1

√x2 + y2

).

and since tan(θ1 + α) = y/x, it follows that

θ1 = tan−1 y

x− α.

The above values for θ1 and θ2 correspond to the elbow-down solution. Theelbow-up solution is given by

θ1 = tan−1 y

x+ α, θ2 = π + β.

If x2+y2 lies outside the range [L1−L2, L1+L2], then no solution exists. Again,the case when (x, y) lies in other quadrants follows straightforwardly from theabove solution for the first quadrant.

This simple motivational example illustrates that for open chains, the inversekinematics problem may have multiple solutions; this is in contrast to the for-ward kinematics, where a unique end-effector displacement T exists for a givenjoint value θ. In fact, three-link planar open chains have an infinite numberof solutions for points (x, y) lying in the interior of the workspace; in this casethe chain possesses an extra degree of freedom, and is said to be kinematicallyredundant.

This chapter begins by considering the inverse kinematics of spatial openchains with six degrees of freedom. A finite number of solutions exists in thiscase, and we first make some simplifying assumptions about the kinematic struc-ture that lead to analytic solutions. As we shall see, these assumptions are notrestrictive, as they accommodate the most commonly used six degree of freedom

6.1. Analytic Inverse Kinematics 161

Figure 6.2: Inverse position kinematics of a 6R elbow-type arm.

open chains. We then specialize the Newton-Raphson method for nonlinear rootfinding to the inverse kinematics problem. The result is an iterative numericalalgorithm that, provided the initial guess is sufficiently close to the true solution,eventually converges to the solution. The chapter concludes with a discussionof pseudo-inverse based inverse kinematics solutions for redundant open chains.

6.1 Analytic Inverse Kinematics

We begin by writing the forward kinematics of a spatial six degree of freedomopen chain in the following product of exponentials form:

T (θ) = e[S1]θ1e[S2]θ2e[S3]θ3e[S4]θ4e[S5]θ5e[S6]θ6M.

Given some X ∈ SE(3), the inverse kinematics problem is concerned withfinding all solutions θ ∈ R6 that satisfy T (θ) = Y . In the following subsectionswe shall make some simplifying assumptions about the kinematic structure ofthe open chain that will lead to analytic solutions for the inverse kinematics.

6.1.1 6R Elbow-Type Arm

We first consider a 6R arm of the elbow type (see Figure 6.2). Such armsare characterized by (i) the last three joint axes intersecting orthogonally ata common point (the wrist center) to form an orthogonal wrist; (ii) the firsttwo axes intersecting orthogonally to form a shoulder joint; and (iii) the elbowjoint axis being parallel with the horizontal shoulder joint axis. Such arms mayalso have an offset at the shoulder (see Figure 6.3). The inverse kinematicsproblem for elbow-type arms can be decoupled into an inverse position andinverse orientation subproblem, which we now discuss.

We first consider the simple case of a zero-offset elbow-type arm. Referringto Figure 6.2 and expressing all vectors in terms of fixed frame coordinates,denote components of the wrist center p ∈ R3 by p = (px, py, pz). Projecting ponto the x-y plane, it can be seen that

θ1 = tan−1 pypx,


(a) Elbow arm withoffset.

(b) Kinematic diagram.

Figure 6.3: A 6R elbow-type arm with a shoulder offset.

Figure 6.4: Singular configuration of the zero-offset 6R elbow-type arm.

where the atan2() function can be used instead of tan−1. Note that a secondvalid solution for θ1 is given by

θ1 = tan−1 pypx

+ π,

provided that the original solution for θ2 is replaced by π − θ2. As long aspx, py 6= 0, both these solutions are valid. When px = py = 0 the arm is in asingular configuration (see Figure 6.4), and there are infinitely many possiblesolutions for θ1.

If there is an offset d1 6= 0 as shown in Figure 6.3, then in general there will betwo solutions for θ1, denoted the right and left arm solutions (Figure 6.3). As

seen from the figure, θ1 = φ−α, where φ = tan−1( pypx ) and α = tan−1(√r2−d21d1

) =


Figure 6.5: Four possible inverse kinematics solutions for the 6R elbow typearm with shoulder offset.

tan−1(√p2x+p2y−d21d1

). The second solution is given by

θ1 = tan−1(pypx

)− tan−1(−√p2x + p2

y − d21

d1)

Determining angles θ2 and θ3 for the elbow manipulator now reduces to theinverse kinematics problem for a planar two-link chain:

cos θ3 =r2 + s2 − a2

2 − a23

2a2a3

=p2x + p2

y + (p2z − d1)2 − a2

2 − a23

2a2a3

If we let cos θ3 = D, then θ3 is given by

θ3 = tan−1(±√

1−D2

D)

θ2 can be obtained in a similar fashion as

θ2 = tan−1(s

r)− tan−1(

a3s3

a2 + a3c3)

= tan−1(pz − d1√p2x + p2

y

)− tan−1(a3s3

a2 + a3c3)


Figure 6.6: A 6R spatial open chain of the generalized elbow type.

The two solutions for θ3 correspond to the well-known elbow up and elbowdown configurations for the two-link planar arm. In general, an elbow-type armwith an offset will have four solutions to the inverse position problem, as shownin Figure 6.5; the upper postures are called left-arm solutions (elbow-up andelbow-down), while the lower postures are called right-arm solutions (elbow-upand elbow-down).

We now solve the inverse orientation problem, i.e., finding values for (θ4, θ5, θ6)given the end-effector frame orientation. This is completely straightforward:since the final three joints form a 3R wrist with orthogonal axes, the joint val-ues can be determined via an appropriate set of Euler angles as discussed inChapter 3 (e.g., ZYX, ZYZ, depending on how the final three joint axes arealigned when in the zero position).

6.1.2 Generalized 6R Elbow-Type Arms

We now relax some of the assumptions made for the 6R elbow-type arm: ageneralized 6R elbow-type arm is characterized by (i) the first two jointaxes intersecting orthogonally, and (ii) the last three joint axes intersectingorthogonally at a common point. Referring to Figure 6.6, place the fixed frameorigin at the intersection of joint axes 1 and 2, and let rw ∈ R3 be the fixedframe representation of the point of intersection of the final three axes. Theassumptions about the joint axes then lead to the following relations among thejoint screws Si = (ωi,−ωi × ri), i = 1, . . . , 6, where ri denotes a reference pointon axis i:

• ωT1 ω2 = 0;

• ωT4 ω5 = 0 and ωT5 ω6 = 0.

The inverse kinematics problem can now be stated as finding solutions θ to

e[S1]θ1e[S2]θ2e[S3]θ3e[S4]θ4e[S5]θ5e[S6]θ6 = XM−1, (6.2)


where

S1 = (ω1, 0)S2 = (ω2, 0)S3 = (ω3,−ω3 × r3)S4 = (ω4,−ω4 × rw)S5 = (ω5,−ω5 × rw)S6 = (ω6,−ω6 × rw),

and the right-hand side XM−1 is given; denote this known quantity by X1 =XM−1. Solving the inverse kinematics then proceeds in three steps:

Step 1: Solve for θ3

We first multiply both sides of Equation (6.2) by rw (here multiplication of avector by a homogeneous transformation is understood in the usual sense, i.e.,

Trw = Rrw + p, T =[R p0 1

].

The net effect is to consecutively apply the screw motions defined by e[S6]θ6 ,e[S5]θ5 , and e[S4]θ4 to rw. However, since all three of these screw motions arezero pitch (all joints are revolute), and rw is a point lying on all three screwaxes, it follows that

e[S4]θ4e[S5]θ5e[S6]θ6rw = rw.

We are thus left with

e[S1]θ1e[S2]θ2e[S3]θ3rw = X1rw = p1, (6.3)

where the vector p1 = X1rw is known.Now take the norm of both sides of (6.3). Since both e[S1]θ1 and e[S2]θ2 are

pure rotations, and the general identity ‖Rv‖ = ‖v‖ holds for any rotation Rand vector v, Equation (6.3) becomes

‖e[S3]θ3rw‖ = ‖p1‖.

This is a problem of the general form

‖e[S]θp‖ = c,

where S = (ω,−ω × r), p ∈ R3, and the scalar c > 0 are known, and theobjective is to find all values of θ ∈ [0, 2π] satisfying this equality. solve thisproblem. Referring to Figure 6.7, define the vectors w, u, v ∈ R3 according to

z = e[S]θp

u = p− rv = w − r,


Figure 6.7: Solving ‖e[S]θp‖ = c for θ.

with ‖z‖ = c given. We shall now project the vectors p, u, v, r, and z onto theplane normal to the screw axis and containing p: define these respectively to be

pproj = p− (pTω)ωuproj = u− (uTω)ωvproj = v − (vTω)ωrproj = r − (rTω)ωzproj = z − (zTω)ω.

Note that uproj and qproj are known a priori. From the figure it can be seenthat

‖(zTω)ω‖ = ‖(pTω)ω‖ = |pTω|.Since zproj = z − (zTω)ω and ‖z‖2 = c2, it follows that

‖zproj‖2 = c2 − (pTω)2,

which is also known a priori. Let us first find the angle ψ = θ + φ, where φ isdefined as indicated in the figure. Since

uTproj(−qproj) = ‖uproj‖ · ‖qproj)‖ cos(θ + φ) (6.4)uproj × (−qproj) = ω (‖uproj‖ · ‖qproj‖ sin(θ + φ)) , (6.5)

from the latter equality it follows that

ωT (uproj × (−qproj)) = ‖uproj‖‖qproj‖ sin(θ + φ). (6.6)


Figure 6.8: Solving e[ω1]θ1e[ω2]θ2p2 = p1 for θ1 and θ2.

From Equations (6.4) and (6.6) we have

ψ = tan−1

(ωT (uproj × qproj)

uTprojqproj

). (6.7)

We now determine φ from the law of cosines: referring to the inset of Figure 6.7,

‖rproj‖2 + ‖vproj‖2 − 2‖rproj‖ · ‖vproj‖ cosφ = ‖zproj‖2.

Since ‖zproj‖2 = c2 − (pTω)2 is known, and ‖vproj‖ = ‖uproj‖ is also known, φcan be determined as follows:

φ = cos−1

(‖rproj‖2 + ‖vproj‖2 − ‖zproj‖2

2‖rproj‖ · ‖vproj‖

). (6.8)

From the figure it should be apparent that there can be up to two solutions forθ:

θ = ψ ± φ.

If φ = 0, the two solutions collapse to a single solution, while a solution doesnot exist in the event that φ does not exist.

Step 2: Solve for θ1 and θ2

Having solved for θ3, Equation (6.3) can be written

e[S1]θ1e[S2]θ2p2 = p1, (6.9)


where p2 = e[S3]θ3rw and p1 are both known. Observe that S1 = (ω1, 0) andS2 = (ω2, 0) are both pure rotations, and ω1 and ω2 are orthogonal to eachother. Thus, only the rotation component of (6.9) needs to be considered:

e[ω1]θ1e[ω2]θ2p2 = p1. (6.10)

Referring to Figure ??, clearly a necessary condition for a solution (θ1, θ2) toexist is that ‖p1‖ = ‖p2‖. Assuming this is the case, the solutions are thenmarked by the intersection of the two circles indicated in Figure 6.8. In thegeneral case there can be up to two solutions, with one or no solution also apossibility.

Assuming a solution exists, let z ∈ R3 be the vector p2 rotated about ω2

by angle θ2. z can also be obtained by rotating p1 about ω1 by an angle −θ1.Mathematically,

z = e[ω2]θ2p2 = e−[ω1]θ1p1.

Clearly {ω1, ω2, ω1 × ω2} forms an orthonormal basis for R3. Further observethat the ω1 component of z is the same as the ω1 component of p1, and theω2 component of z is the same as the ω2 component of p2). z can therefore beexpressed in terms of this orthonormal basis as

z = (pT1 ω1)ω1 + (pT2 ω2)ω2 ± c(ω1 × ω2)

for some scalar constant c ≥ 0. The length ‖z‖ is then, straightforwardly,

‖z‖ = (pT1 ω1)2 + (pT2 ω2)2 + c2.

Since z is also a rotated version of p2 (and also of p1), it follows that ‖z‖ =‖p2‖ = ‖p1‖. Solving the above for c2, z can now be written

z = (pT1 ω1)ω1 + (pT2 ω2)ω2 ±√‖p2‖2 − (pT1 ω1)2 − (pT2 ω2)(ω1 × ω2).

If c = 0 then there exists a unique solution (θ1, θ2), while if c does not exist,then no solution (θ1, θ2) exists.

Having found up to two possible solutions for z, what remains is to find θ1

and θ2 for each z. This is relatively straightforward: letting u1 and z1 respec-tively be the projections of p1 and z onto circle 1, and u2 and z2 respectivelybe the projections of p2 and z onto circle 2, it follows that

θ1 = cos−1(uT1 z1)θ2 = cos−1(uT2 z2).

Step 3: Solve for θ4, θ5, and θ6

Having found θ1, θ2, and θ3, it remains to solve for θ4, θ5, and θ6. We have

e[S4]θ4e[S5]θ5e[S6]θ6 = e−[S3]θ3e−[S2]θ2e−[S1]θ1XM−1 (6.11)= X2,


where the right-side X2 is now known. Recall that ωT4 ω5 = 0 and ωT5 ω6 = 0;this implies that ω4 and ω6 are either orthogonal or parallel. Assume for thetime being that ω6 is orthogonal to ω4, or more precisely, ω6 = ω4 × ω5 (theparallel case will be considered later). Define the transformation

Tw =[Rw rw0 1

], Rw =

[ω6 −ω5 ω4

]∈ SO(3).

Multiplying both sides of (6.11) by T−1w leads to

T−1w e[S4]θ4e[S5]θ5e[S6]θ6 = T−1

w X2

eT−1w [S4]Twθ4eT

−1w [S5]Twθ5eT

−1w [S6]Twθ6 = T−1

w X2Tw.

Noting that T−1w [Si]Tw is the 4×4 matrix representation of the adjoint mapping

AdT−1w

(Si), it can be verified by calculation that

AdT−1w

(S6) =

0 0 0 00 0 −1 00 1 0 00 0 0 0

AdT−1w

(S5) =

0 0 1 00 0 0 0−1 0 0 0

0 0 0 0

AdT−1w

(S4) =

0 −1 0 01 0 0 00 0 0 00 0 0 0

.As long as the translational component of T−1

w X1Tw is zero, the solution (θ4, θ5, θ6)can now be obtained from the following:

Rot(z, θ1) · Rot(y, θ2) · Rot(x, θ3) = R,

where R is the rotational component of T−1w X1Tw. The solution (θ4, θ5, θ6)

therefore corresponds to the ZY X Euler angles that parametrize R:

θ5 = atan2(−r31,±√r112 + r2

21)

θ4 = atan2(r21, r11)θ6 = atan2(r32, r33),

where rij denotes the ij-th entry of R. Recall that there are two solutionsdetermined by the choice of sign for θ5.

The above derivation can be repeated for the case when ω6 and ω4 areparallel, in which case the solutions (θ4, θ5, θ6) will now correspond to the ZYZEuler angles.


Figure 6.9: The first three joints of a Stanford-type arm.

Like the earlier 6R elbow-type arm, since there are up to two possible so-lutions for θ3, and also for (θ1, θ2) and (θ4, θ5, θ6), for the generalized elbowtype robot arm one can expect up to 2 × 2 × 2 = 8 possible inverse kinematicsolutions.

6.1.3 Stanford-Type Arms

If the elbow joint in a 6R elbow-type arm is replaced by a prismatic joint asshown in Figure 6.9, we then have a Stanford-type arm. Here we consider theinverse position kinematics for the arm of Figure 6.9; the inverse orientationkinematics is identical to that for the elbow-type arm and is not repeated here.

The first joint variable θ1 an be found in a similar fashion to the elbow-typearm: θ1 = tan−1( pypx ) (provided that px and py are not both zero). θ2 is thenfound from Figure 6.9 to be

θ2 = tan−1(s

r)

where r2 = p2x + p2

y and s = pz − d1. Similar to the elbow-type arm, a secondsolution for θ1, θ2 is given by

θ1 = π + tan−1(pypx

)

θ2 = π − tan−1(pypx

)

The translation distance d3 is found from the relation

(d3 + a2)2 = r2 + s2

6.2. Numerical Inverse Kinematics 171

Figure 6.10: The Newton-Raphson method for nonlinear root-finding.

as

d3 =√r2 + s2

=√p2x + p2

y + (pz − d1)2 − a2

Ignoring the negative square root solution for d3, we obtain two solutions tothe inverse position kinematics as long as the wrist center p does not intersectthe z-axis of the fixed frame. If there is an offset, then as in the case of theelbow-type arm there will be a left and right arm solution.

If the elbow joint in the generalized 6R elbow-type arm is replaced by aprismatic joint, the resulting arm is then referred to as a generalized Stanford-type arm. The inverse kinematics proceeds in the same way as for the general-ized elbow-type arm; the only difference occurs in the first step (obtaining θ3).The screw vector for the third joint now becomes S3 = (0, v3), with ‖v3‖ = 1,and θ3 is found by solving the equation

‖e[S3]θ3p‖ = c,

for some given p ∈ R3 and nonnegative positive scalar c. The above equationreduces to solving the following quadratic in θ3:

θ23 + 2(pT v3)θ3 + (‖p‖2 − c2) = 0.

Imaginary, as well as negative solutions, naturally should be excluded.

6.2 Numerical Inverse Kinematics

In cases where the inverse kinematics equations do not admit analytic solutions,one must resort to numerical methods. Even in cases where an analytic solution


exists, numerical methods are often used to improve the accuracy of these so-lutions. For example, in a generalized elbow-type arm, the last three axes maynot exactly intersect at a common point, and the shoulder joint axes may not beexactly orthogonal. In such cases, rather than throw away any analytic inversekinematic solutions that are available, such solutions can be used as the initialpoint in an iterative procedure for numerically solving the inverse kinematics.

There exist a variety of iterative methods for finding the roots of a nonlinearequation, and our aim is not to discuss these in detail—any text on numericalanalysis will cover these methods in depth—but rather to develop ways in whichto transform the inverse kinematics equations such that they are amenable toexisting numerical methods.

However, it is useful to review at least one fundamental approach, theNewton-Raphson method. Suppose we wish to find the roots of the nonlin-ear equation f(x) = 0, where f : Rn → Rn is assumed twice differentiable. Tofirst-order, f(x) can be approximated about a point x0 ∈ Rn by

f(x) ≈ f(x0) +∂f

∂x(x0)(x− x0) (6.12)

where

∂f

∂x(x0) =

∂f1∂x1

(x0) · · · ∂f1∂xn

(x0)...

. . ....

∂fn∂x1

(x0) · · · ∂fn∂xn

(x0)

∈ Rn×n. (6.13)

Setting f(x) = 0 and assuming ∂f∂x (x0) is invertible, we have

x = x0 −(∂f

∂x(x0)

)−1

f(x0). (6.14)

If f(x) is approximated in a neighborhood of x0 by a linear function, then theroot is given by the expression for x above.

The basic idea behind the Newton-Raphson method is make successive linearapproximations of f(x) as indicated (see Figure 6.10). Given an initial guess x0,we approximate f(x) in a neighborhood of x0 by a linear function, and find theroot; call this root x1. f(x) is then linearly approximated in a neighborhoodof x1 as before, and the root determined; call this root x2. This iterationis successively repeated until some convergence criterion is satisfied, e.g., thedifference ‖xk+1− xk‖ < ε for some threshold ε ∈ R, or the function value itselfis below some threshold ‖f(xk+1)‖ < ε.

There exist many refinements to the above basic Newton-Raphson method,which we do not discuss here. Our focus will be to transform the inverse kine-matics equations in a form that is amenable to numerical solution. For a sixdegree-of-freedom open chain, we will first have to reformulate the kinematicequations in the form f : R6 → R6. There are several ways to do this.

Given T (θ) = X, where θ ∈ R6, T (·) represents the forward kinematics,and X ∈ SE(3) is the given end-effector frame location, we first transform thisequation to either T (θ)X−1 = I or X−1T (θ) = I. One way to reduce this to a

6.2. Numerical Inverse Kinematics 173

set of six independent equations is to take the matrix logarithm of both sides,i.e.,

logX−1T (θ) = G(θ) = 0. (6.15)

It quickly becomes apparent that the G(θ) obtained via the logarithm will bequite complicated. In fact, the situation does not improve measurably if otherparametrizations, e.g., using Euler angles for the rotation component, are used.Also, any numerical method will at some point need to differentiate G(θ), if notanalytically, then via a finite difference approximation scheme.

One way to avoid these difficulties is to attempt to directly formulate aNewton-Raphson algorithm for open chain kinematics that explicitly uses theJacobian. We now do so for the body Jacobian. Recall that the body Jacobianis obtained from

[Vb] = T−1T = [Jb(θ)θ],

where Jb(θ) ∈ R6×6. Let us write these two equations in the following differentialform:

[Vb]4t = T−14T = [Jb(θ)4θ]. (6.16)

Suppose we have some initial guess θ0 ∈ R6. Since our objective is to find, for agiven X ∈ SE(3), θ that satisfies X = T (θ), we set T−1 in the previous equationto X−1, and 4T to X − T (θ0). Making these substitutions, [Vb]4t = T−14Tcan be written

[Vb]4t = X−1(X − T (θ0)) = I −X−1T (θ0). (6.17)

Let us further make a first-order approximation to X−1T (θ0) in terms of theexponential map. That is, suppose

X−1T (θ0) = e[S]

= I + [S] +12!

[S]2 + . . . ,

where [S] = logX−∞T (θ′). Ignoring second and higher order terms,

X−1T (θ0) ≈ I + [S],

With these substitutions, Equation (6.16) becomes

Jb(θ)4θ = S, [S] = logX−1T (θ0).

The above suggests the following algorithm for solving T (θ) = X:

(i) Initialization: Given X ∈ SE(3) and initial guess θ ∈ R6.

(ii) While ‖X − T (θ)‖ > ε do:

• Set [S] = logX−1T (θ);

• Solve Jb(θ)4θ = S for 4θ;


• Set θ ← θ +4θ.

The above algorithm could also have been formulated in terms of the spatialJacobian Js(θ); the corresponding numerical inverse kinematics algorithm is asfollows:

(i) Initialization: Given X ∈ SE(3) and initial guess θ ∈ R6.

(ii) While ‖X − T (θ)‖ > ε do:

• Set [S] = log T (θ)X−1;

• Solve Js(θ)4θ = S for 4θ;• Set θ ← θ +4θ.

6.3 Inverse Kinematics of Redundant Open Chains

We close this chapter with a brief look at the inverse kinematics problem for re-dundant open chains. A spatial open chain is said to be kinematically redundantif it has mobility greater than six; in this case the inverse kinematics problemX = T (θ), where dim(θ) > 6, has in general an infinite number of solutions forany given X. An analytical characterization of all possible solutions is difficult,even with simplifying assumptions on the kinematic structure. Our focus inthis section will therefore be on solving the differential version of the inversekinematics problem: given a desired end-effector spatial velocity V, the goal isto find a suitable joint velocity θ that satisfies V = J(θ)θ (this equation canbe expressed in either space or body frame coordinates). Because J(θ) ∈ R6×n

with n > 6, there exists an (n− 6)-parameter family of solutions for θ.One solution for θ that is both convenient and physically meaningful is the

minimum norm solution, which we now describe. Among the many solutionsto V = J(θ)θ, suppose we seek the θ with the smallest norm ‖θ‖ that satisfiesV J(θ)θ, where the norm ‖ · ‖ is defined in the more general form

‖θ‖ =√θTQθ

for some symmetric and positive definite matrix Q ∈ Rn×n. For example, ifall of the joint actuators are identical, a reasonable choice for Q is the identitymatrix. If the actuators have different sizes (in the sense of maximum attainablevelocities, for example), then Q can be chosen to be a diagonal matrix, witheach diagonal entry proportional to the size of its corresponding actuator.

The minimum-norm solution is well-known (consult any linear algebra text-book):

θ = QJT (JQJT )−1V.

That this θ satisfies J(θ)θ = V can be verified by direct substitution. Notethat JQJT ∈ R6×6, so that as long as J(θ) is of maximal rank six (which isequivalent to the robot arm not being in a kinematically singular configuration),

6.4. Summary 175

JQJT will always be nonsingular. If joint values θ(t) are required, the followingiteration can be used:

θ(t+4t) = θ(t) +4t(QJ(θ(t))T (J(θ(t))QJ(θ(t))T )−1

)V(t),

where V(t) is the desired spatial velocity profile for the end-effector frame. Thisinverse kinematics solution is useful when desired trajectories for the end-effectorframe, X(t) ∈ SE(3), are given over some time interval [t0, t1], and we wish todetermine the corresponding joint trajectory θ(t).

6.4 Summary

• Given a spatial open chain with forward kinematics T (θ), θ ∈ Rn, in theinverse kinematics problem one seeks to find, for some given X ∈ SE(3),solutions θ that satisfy X = T (θ). Unlike the forward kinematics, theinverse kinematics problem can possess multiple solutions, or no solutionsin the event that X lies outside the task space. For a spatial open chain,n ≤ 6 typically leads to a finite number of inverse kinematic solutions,while n > 6 leads to an infinite number of solutions.

• The inverse kinematics can be solved analytically for a large class of openchains that possess some degree of structure. One important class is thegeneralized 6R elbow-type arm; such an arm consists of a 3R orthogonalaxis wrist connected to a 2R orthogonal axis shoulder by an elbow joint.Geometric algorithms have been developed for this class of arms thatexploits the product of exponentials forward kinematics representation.Further assumptions on the joint axes, e.g., joint axes 2 and 3 are alwaysparallel, lead to simpler analytic forms for the inverse kinematics.

• Another class of open chains that admit analytic inverse kinematic solu-tions are Stanford-type arms; these arms are obtained by replacing theelbow joint in the generalized 6R elbow-type arm by a prismatic joint.Geometric inverse kinematic algorithms similar to those for elbow-typearms have also been developed.

• Iterative numerical methods are used in cases where analytic inverse kine-matics solutions are not available. These typically involve solving the in-verse kinematics equations through an iterative procedure like the Newton-Raphson method, and require an initial point to be supplied. The perfor-mance of the iterative procedure depends to a large extent on the quality ofthe initial point, and only one inverse kinematic solution is produced periteration. An iterative procedure based on the Jacobian of the forwardkinematics has been developed for general six degree-of-freedom spatialopen chains.

• In cases where n > 6, typically there exists an infinite number of solu-tions to the inverse kinematics. For such kinematically redundant open


chains, it is useful to consider the differential inverse kinematics problem:given some desired end-effector velocity profile V(t) and a current postureθ ∈ Rn, find the joint velocity vector θ that satisfies V = J(θ)θ. Whilean infinite number of solutions θ also exists in this case, the minimumnorm solution is one particularly useful and convenient solution: it is thesmallest joint velocity θ, in the sense of minimizing θTQθ for some givensymmetric positive-definite Q ∈ Rn×n, that satisfies V = J(θ)θ. Theexplicit form of the solution θ is given by

θ = QJT (JQJT )−1V.

Based on the above solution to the differential inverse kinematics, an iterativeprocedure for solving the inverse kinematics at discrete times can be obtainedas follows:

θ(t+4t) = θ(t) +4t(QJ(θ(t))T (J(θ(t))QJ(θ(t))T )−1

)V(t).

One issue to be aware of in inverse kinematics schemes like the above is re-peatability: when the end-effector frame is a periodic closed path, in principleone would also want the robot arm posture to follow a periodically repeatingtrajectory. Any iterative inverse kinematics scheme for redundant arms mustsatisfy a further set of conditions to be repeatabe.


The inverse kinematics of the most general 6R open chain is known to haveup to 16 solutions; this result was first proved by Lee and Liang [15], and alsoby Raghavan and Roth [33]. Iterative numerical procedures for finding all six-teen solutions of a general 6R open chain are reported in [18]. A summaryof inverse kinematics methods for kinematically redundant robot arms are dis-cussed in [37]. The repeatability conditions for kinematically redundant inversekinematics schemes are examined in [36].

6.6. Exercises 177

Figure 6.11: A 3R wrist.

6.6 Exercises

1. The 3R orthogonal axis wrist mechanism of Figure 6.11 is shown in its zeroposition, with joint axes 1 and 3 collinear.(a) Ignoring position and considering only the orientation of the end-effectorframe, find all kineamtic singularities of the wrist mechanism.(b) Given a desired wrist orientation R ∈ SO(3), derive an iterative numericalprocedure for solving its inverse kinematics.(c) Assuming static equilibrium, suppose that at the zero position we wish togenerate a moment (0, 1, 0) (in fixed frame coordinates) at the end-effector.What joint torques should be applied?

2. The 3R nonorthogonal chain of Figure 6.12 is shown in its zero position.(a) Derive a numerical procedure for solving the inverse position kinematicsnumerically; that is, given some end-effector position p as indicated in the figure,find (θ1, θ2, θ3).(b) Given an end-effector orientation R ∈ SO(3), find all inverse kinematicsolutions (θ1, θ2, θ3).

3. The RRP open chain of Figure 6.13 is shown in its zero position. Joint axes1 and 2 intersect at the fixed frame origin, and the end-effector frame origin pis located at (0, 1, 0) when the robot is is its zero position.


Figure 6.12: A 3R nonorthogonal chain.

Figure 6.13: An RRP open chain.

(a) Suppose θ1 = 0. Solve for θ2 and θ3 when the end-effector frame origin p isat (−6, 5,

√3).

(b) If joint 1 is not fixed to zero but instead allowed to vary, find all inversekinematic solutions (θ1, θ2, θ3) for the same p given in part (a).

6.6. Exercises 179

Figure 6.14: A 6R open chain of the elbow type.

Figure 6.15: An inverse elbow type robot arm.

4. Find the inverse kinematics solutions when tool frame {T} of the 6R openchain of the elbow type shown in Figure 6.14 is set to {T ′} as shown. Theorientation of {T} at the zero position is the same as that of the fixed frame{S}, and T ′ is the result of a pure translation of T along the y-axis.

5. (a) Solve the inverse position kinematics (you do not need to solve the ori-entation kinematics) of the inverse elbow type robot arm shown in Figure 6.15.(b) Determine the spatial Jacobian Js(theta) of the inverse elbow robot arm.(c) Find as many kinematic singularities of the inverse elbow arm that you can,and for each singularity, describe the directions in which the end-effector losesdegrees of freedom of motion.

Chapter 7

Kinematics of ClosedChains

Any kinematic chain that contains one or more loops is called a closed chain.Several examples of closed chains were encountered in Chapter 2, from the pla-nar four-bar linkage to spatial mechanisms like the Stewart-Gough platform. Inthis chapter we shall analyze the kinematics of closed chains, paying special at-tention to a class of closed chains that we shall refer to as parallel mechanisms;these are closed chains consisting of a fixed and moving platform connected bya set of “legs”; these legs are mostly open chains, but sometimes can themselvesbe closed chains.

Figures 7.1-7.3 depict some well-known parallel mechanisms. The Stewart-Gough Platform is a six degree of freedom mechanism, used widely as botha motion simulator and six-axis force-torque sensor. It is typically realizedas either a 6 × UPS or 6 × SPS platform; note that the additional torsionalrotations of each of the six legs in the 6 × SPS platform have no effect on themoving platform. When used as a force-torque sensor, the six prismatic jointsexperience internal linear forces whenever any external force is applied to themoving platform; by measuring these internal linear forces one can estimate theapplied external force. The Delta robot is a three degree of freedom mechanismthat has the unusual feature of the moving platform always remaining parallelto the fixed platform. Because the three actuators are all attached to the threerevolute joints of the fixed upper platform, the moving parts are relatively light;this allows the Delta to achieve very fast motions. The Eclipse mechanism isanother six degree of freedom parallel mechanism whose moving platform iscapable of ±90◦ orientations with respect to ground, and also of rotating 360◦

about the vertical axis.Closed chains admit a much greater variety of designs than open chains,

and not surprisingly their kinematic analysis is considerably more complicated.This can be traced to two defining features of closed chains: (i) the configu-ration space is curved (e.g., a multidimensional surface embedded in a higher-

181

182 Kinematics of Closed Chains

{F}

{T} bi

a i

d i

Ai

Bi

p

Figure 7.1: The Stewart-Gough platform.

Figure 7.2: The Delta robot.

dimensional vector space), and (ii) not all of the joints are actuated. The pres-ence of such non-actuated, or passive joints, together with the fact that thenumber of actuated joints may deliberately exceed the mechanism’s kinematic

183

Figure 7.3: The Eclipse mechanism.

degrees of freedom—such mechanisms are said to be redundantly actuated—makes not only the position and differential kinematics analysis more challeng-ing, but also introduces new types of singularities not witnessed in open chains.

Recall also that for open chains, the kinematic analysis proceeds in a moreor less straightforward fashion with the formulation of the forward kinematics(e.g., via the product of exponentials formalism) followed by that of the inversekinematics. For general closed chains it is usually difficult to obtain an ex-plicit set of equations for the forward kinematics in the form X = T (θ), whereX ∈ SE(3) is the end-effector frame and θ ∈ Rn are the joint coordinates.The most effective approaches for closed chain kinematic analysis are based ona collection of tools and methodologies that exploit as much as possible anykinematic symmetries and other special features of the mechanism.

For this reason we shall proceed in this chapter with a series of case stud-ies involving some well-known parallel mechanisms, and eventually build up arepetoire of kinematic analysis tools and methodologies that can be synthesizedto handle more general closed chains. We shall consider only parallel mecha-nisms that are exactly actuated, i.e., the number of actuated degrees of freedomis equal to the mechanism’s kinematic mobility. Methods for the forward andinverse position kinematics of parallel mechanisms are discussed, followed bythe characterization and derivation of the constraint Jacobian, and the Jaco-bians of both the inverse and forward kinematics. The chapter concludes withan examination of the various kinematic singularities that can arise in closedchains.


Figure 7.4: A three degree-of-freedom 3×RPR planar parallel mechanism.

7.1 Inverse and Forward Kinematics

This section examines methods for the inverse and forward kinematics of closedchains. Rather than attempt to develop a general methodology applicable to alltypes of closed chains, we consider two case studies, the 3×RPR planar parallelmechanism, and its spatial counterpart, the 3× SPS Stewart-Gough platform.The analysis of these two mechanisms draws upon some reduction techniquesthat result in a reduced form of the governing kinematic equations. We brieflydescribe how these methods can be generalized to the analysis of more generalparallel mechanisms.

7.1.1 3×RPR Planar Parallel Mechanism

The first example we consider is the planar 3×RPR parallel mechanism shownin Figure 7.4. It is easily verified from the planar version of Gruebler’s formulathat this mechanism has mobility three. Assign a fixed frame {s} and end-effector frame {b} as shown. Typically the three prismatic joints are actuated;denote the lengths of each of the three legs by si, i = 1, 2, 3. The forwardkinematics problem is to determine, from given values of s = (s1, s2, s3), theend-effector frame’s position and orientation.

Let ~p be the vector from the origin of the {s} frame to the origin of the {b}frame. Let φ denote the angle measured from the x axis of the {s} frame tothe {x} axis of the {b} frame. Further define the vectors ~ai, ~bi, ~di, i = 1, 2, 3 asshown in the figure. From these definitions, clearly

~di = ~p+~bi − ~ai, (7.1)

7.1. Inverse and Forward Kinematics 185

for i = 1, 2, 3. Let [pxpy

]= ~p in {s} frame coordinates[

aixaiy

]= ~ai in {s} frame coordinates[

dixdiy

]= ~di in {s} frame coordinates[

bixbiy

]= ~bi in {b} frame coordinates.

Note that the vectors (aix, aiy), (bix, biy), i = 1, 2, 3 are constant, and thatwith the exception of (bix, biy), all other vectors are expressed in {s} framecoordinates. To express Equation (7.1) in terms of {s} frame coordinates, itis first necessary to find the {s} frame representation of the vector ~bi. This isstraightforward: defining

Rsb =[

cosφ − sinφsinφ cosφ

],

it now follows that[dixdiy

]=[pxpy

]+[

cosφ − sinφsinφ cosφ

] [bixbiy

]−[aixaiy

],

for i = 1, 2, 3. Also, since s2i = d2

ix + d2iy, we have

s2i = (px + bix cosφ− biy sinφ− aix)2

+(py + bix sinφ+ biy cosφ− aiy)2,

for i = 1, 2, 3.Formulated as above, the inverse kinematics is trivial to compute: given

values for (px, py, φ), the leg lengths (s1, s2, s3) can be directly calculated fromthe above equations (negative values of si in most cases will not be physicallyrealizable, and can be ignored). The forward kinematics problem, in contrast, isnot trivial: here the objective is to determine, for given values of (s1, s2, s3), theend-effector frame’s position and orientation (px, py, φ). The following tangenthalf-angle substitution, widely used in kinematic analysis, transforms the abovethree equations into a system of polynomials in the newly defined scalar variablet:

t = tanφ

2

sinφ =2t

1 + t2

cosφ =1− t2

1 + t2.


After considerable algebraic manipulation, this system of polynomials can even-tually be reduced to a single sixth-order polynomial in t, which effectively showsthat the 3×RPR mechanism may have up to six forward kinematics solutions(showing that six real solutions are possible requires further verification, whichwe do not pursue here).

7.1.2 Stewart-Gough Platform

We now examine the inverse and forward kinematics of the 6 × SPS Stewart-Gough platform of Figure 7.1. In this design, the fixed and moving platformsare connected by six serial SPS structures, with the spherical joints passive,and the prismatic joints actuated. The derivation of the kinematic equationsclosely parallels that of our earlier planar 3 × RPR mechanism. Let {s} and{b} denote the fixed and end-effector frames, respectively, and let ~di be thevector directed from joint Ai to joint Bi. Referring to Figure 7.1, we make thefollowing definitions:

• p ∈ R3 = ~p in {s} frame coordinates;

• ai ∈ R3 = ~ai in {s} frame coordinates;

• bi ∈ R3 = ~bi in {b} frame coordinates;

• di ∈ R3 = ~di in {s} frame coordinates.

• R ∈ SO(3) = orientation of {b} as seen from {s}.

In order to derive the kinematic constraint equations, note that vectorially,

~di = ~p+~bi − ~ai, i = 1, . . . , 6.

Writing the above equations explicitly in {s} frame coordinates,

di = p+Rbi − ai, i = 1, . . . , 6.

Denoting the length of leg i by si, we have

s2i = dTi di = (p+Rbi − ai)T (p+Rbi − ai),

for i = 1, . . . , 6. Observe that ai and bi as defined above are all known con-stant vectors. Having written the constraint equations in this form, the inversekinematics now becomes straightforward: given p and R, the six leg lengths si,i = 1, . . . , 6 can be evaluated directly from the above equations (negative valuesof si in most cases will not be physically realizable, and can be ignored).

The forward kinematics is not as straightforward. Here we are given each ofthe leg lengths si, i = 1, . . . , 6, and must solve for p ∈ R3 and R ∈ SO(3). Thesix constraint equations, together with the rotation matrix constraint RTR = I,constitute a set of twelve equations in twelve unknowns. Several methods existfor finding all solutions to such a set of polynomial equations, e.g., methods

7.1. Inverse and Forward Kinematics 187

Figure 7.5: A general spatial parallel mechanism.

based on dialytic elimination, Grobner bases, etc. Of particular note is the workof Raghavan and Roth [?], who show that there can be at most forty solutions tothe forward kinematics, and Husty [?], who develops a computational algorithmfor finding all forty solutions analytically.

7.1.3 General Parallel Mechanisms

For both the 3 × RPR mechanism and Stewart-Gough Platform, we were ableto exploit certain features of the mechanism that resulted in a reduced set ofequations; for example, in the case of the Stewart-Gough Platform, the fact thateach of the “legs” can be modelled as straight lines considerably simplified theanalysis. In this brief section we consider the more general case where the legshave the structure of an arbitrary open chain.

Consider such a parallel mechanism as shown in Figure 7.5; here the fixedand moving platforms are connected by three open chains. Denote the forwardkinematics of the three chains by T1(θ), T2(φ), and T3(ψ), respectively, whereθ ∈ Rm, φ ∈ Rn, and ψ ∈ Rp. The loop closure conditions can be written

T1(θ) = T2(φ) (7.2)T2(φ) = T3(ψ). (7.3)

Equation 7.2 and 7.3 each consists of 12 equations (9 for the rotation componentand 3 for the position component), 6 of which are independent (recall that thenine equations for the rotation component can be reduced to a set of three


independent equations from the rotation matrix constraint, i.e., RTR = I);there are thus 24 equations, 12 of which are independent, with n + m + punknown variables, and the mobility of the mechanism is d = 12− (n+m+ p).

In the forward kinematics problem, given values for d of the joint variables(θ, φ, ψ), Equations 7.2 and 7.2 can be solved for the remaining joint variables;note that multiple solutions will be likely. Once the joint values for any one ofthe open chain legs are known, the forward kinematics of that leg can then beevaluated to determine the forward kinematics of the closed chain.

In the inverse kinematics problem, we are given the end-effector frame dis-placement T ∈ SE(3). Setting T = T1 = T2 = T3, the objective is to solveEquations 7.2 and 7.2 for all the joint variables (θ, φ, ψ). As hinted by the casestudies, for most parallel mechanisms there are often features of the mechanismthat can be exploited to eliminate some of these equations, and to simplify theminto a reduced form.

7.2 Differential Kinematics

We now consider the differential kinematics of parallel mechanisms. Unlike dif-ferential kinematics for open chains, in which the objective was to relate theinput joint velocities to the spatial velocity of the end-effector frame, the anal-ysis for closed chains is complicated by the fact that not all of the joints areactuated. Only the actuated joints can be prescribed input velocities; the veloc-ities of the remaining passive joints must then be determined from the kinematicconstraint equations. These passive joint velocities are usually required to even-tually determine the spatial velocity of the closed chain’s end-effector frame.

For open chains, the Jacobian of the forward kinematics played a definingrole in both velocity and static analysis. For closed chains, in addition to theforward kinematics Jacobian, the Jacobian defined by the kinematic constraintequations—for this reason we refer to this latter Jacobian as the constraintJacobian—also plays a central role in velocity and static analysis. Much likethe case for the inverse and forward kinematic analysis of parallel mechanisms,often there are features of the mechanism that can be exploited to simplifyand reduce the procedure for obtaining the Jacobians. We therefore begin witha case study of the Stewart-Gough platform, and show that the Jacobian ofthe inverse kinematics can be obtained straightforwardly via static analysis.Velocity analysis for more general parallel mechanisms is then detailed.

7.2.1 Stewart-Gough Platform

Earlier we saw that the inverse kinematics for the Stewart-Gough platform canbe solved analytically; that is, given the end-effector frame orientation R ∈SO(3) and position p ∈ R3, the leg lengths s ∈ R6 can be obtained analytically inthe functional form s = g(R, p). In principle this equation can be differentiatedand manipulated to eventually produce a differential version, e.g.,

s = G(R, p)Vs, (7.4)

7.2. Differential Kinematics 189

where s ∈ R6 denotes the leg velocities, Vs ∈ R6 is the end-effector’s spatialvelocity in fixed frame coordinates, and G(R, p) ∈ R6×6 is the Jacobian of theinverse kinematics. This derivation, however, will likely involve considerablealgebraic manipulation.

Here we take a different approach based on static analysis. Based on the samevirtual work considerations that were used to determine the static relationshipfor open chains, the static relationship for closed chains (expressed in the fixedframe) is also given by τ = JTs Fs, where τ is the vector of input joint torques,Fs is (the fixed frame representation of) the external spatial force applied at theend-effector frame, and Js denotes the space Jacobian of the forward kinematics.

For the Stewart-Gough platform, note that the only forces being applied tothe moving platform occur at the spherical joints. Let

fi = ωiτi

be the three-dimensional linear force applied by leg i, where ωi ∈ R3 is a unitvector indicating the direction of the applied force, and τi ∈ R is the magnitudeof the linear force; we emphasize that fi is expressed in terms of the fixed framecoordinates. The moment generated by fi, denoted mi, is then given by

mi = ri × fi,

where ri ∈ R3 denotes the vector from the fixed frame origin to the point ofapplication of the force (spherical joint i in this case); again, both ri and mi

are expressed in fixed frame coordinates. It is not too difficult to see that thissame moment can also be expressed as

mi = qi × fi,

where qi ∈ R3 denotes the vector from the fixed frame origin to the base of legi, i.e., the joint connecting leg i to the fixed base. Expressing the moment asqi × fi is preferred, since qi as defined is constant.

Combining fi and mi into a six-dimensional spatial force Fi = (mi, fi), theresultant spatial force Fs on the moving platform is then given by

Fs =6∑i=1

Fi =6∑i=1

[ri × fiωi

]τi

=[−ω1 × q1 · · · −ω6 × q6

ω1 · · · ω6

] τ1...τ6

.Since earlier we asserted that the static relationship for the Stewart-Gough

platform is also of the form τ = JTs Fs, based on the previous derivation we canconclude that the inverse Jacobian J−1

s (or equivalently, the Jacobian of theinverse kinematics) is given by

J−1s =

[−ω1 × q1 · · · −ω6 × q6

ω1 · · · ω6

]T.


7.2.2 General Parallel Mechanisms

Because of its kinematic structure, the Stewart-Gough platform lends itself par-ticularly well to a static analysis, as each of the six joint forces are directed alongtheir respective legs. The Jacobian (or more precisely, the inverse Jacobian) cantherefore be derived in terms of the screws associated with each line. In thissection we consider more general parallel mechanisms where a static analysis isnot as straightforward. Using the previous three-legged, three degree-of-freedomspatial parallel mechanism of Figure 7.5 as an example, we illustrate a generalprocedure for determining the forward kinematics Jacobian; generalizing thismethod to arbitrary parallel mechanisms should be completely straightforward.

The mechanism of Figure 7.5 consists of two platforms connected by threelegs, with each leg a five degree of freedom open chain. For the given fixedand end-effector frames as indicated in the figure, we first write the forwardkinematics for the three chains as follows:

T1(θ1, θ2, . . . , θ5) = e[S1]θ1e[S2]θ2 · · · e[S5]θ5M1

T2(φ1, φ2, . . . , φ5) = e[P1]φ1e[P2]φ2 · · · e[P5]φ5M2

T3(ψ1, ψ2, . . . , ψ5) = e[Q1]ψ1e[Q2]ψ2 · · · e[Q5]ψ5M3.

The kinematic loop constraints can be expressed as

T1(θ) = T2(φ) (7.5)T2(φ) = T3(ψ). (7.6)

Taking right differentials of both sides of the above two equations, we have

T1T−11 = T2T

−12 (7.7)

T2T−12 = T3T

−13 . (7.8)

Since TiT−1i = [Vi], where Vi is the spatial velocity of chain i’s end-effector

frame, the above identities can also be expressed in terms of the forward kine-matics Jacobian for each chain:

J1(θ)θ = J2(φ)φ (7.9)J2(φ)φ = J3(ψ)ψ, (7.10)

which can also be rearranged as

[J1(θ) −J2(φ) 0

0 −J2(φ) J3(ψ)

] θ

φ

ψ

= 0. (7.11)

At this point we now rearrange the fifteen joints into those that are actuated,and those that are passive. Let us assume without loss of generality that thethree actuated joints are (θ1, φ1, ψ1). Define the vector of actuated joints qa ∈ R3

7.2. Differential Kinematics 191

and passive joints qp ∈ R12 as

qa =

θ1

φ1

ψ1

, qp =

θ2

...φ5

,and q = (qa, qp) ∈ R15. Equation (7.11) can now be rearranged into the form[

Ha(q) Hp(q)] [ qa

qp

]= 0, (7.12)

or equivalentlyHaqa +Hpqp = 0, (7.13)

where Ha ∈ R12×3 and Hp ∈ R12×12. If Hp is invertible, we have

qp = −H−1p Haqa. (7.14)

Assuming Hp is invertible, once the velocities of the actuated joints are given,the velocities of the remaining passive joints can be obtained uniquely via Equa-tion 7.14.

It still remains to derive the forward kinematics Jacobian with respect tothe actuated joints, i.e., to find Ja(q) ∈ R6×3 satisfying Vs = Ja(q)qa, where Vsis the spatial velocity of the end-effector frame. For this purpose we can use theforward kinematics for any of the three open chains; for example, in terms ofchain 1, J1(θ)θ = Vs, and from Equation (7.14) we can write

θ2 = gT2 qa (7.15)θ3 = gT3 qa (7.16)θ4 = gT4 qa (7.17)θ5 = gT5 qa (7.18)

where each gi(q) ∈ R3, i = 2, . . . , 5, can be obtained from Equation (7.14).Defining the row vector eT1 = (1, 0, 0), the differential forward kinematics forchain 1 can now be written

Vs = J1(θ)

eT1gT2gT3gT4gT5

θ1

φ1

ψ1

. (7.19)

Since we are seeking Ja(q) in Vs = Ja(q)qa, and qTa = (θ1, φ1, ψ1), from theabove it now follows that

Ja(q) = J1(q1, . . . , q5)

eT1

g2(q)T

g3(q)T

g4(q)T

g5(q)T

. (7.20)


Figure 7.6: A planar four-bar linkage and its joint configuration space.

Figure 7.7: A planar five-bar linkage.

The above could also have been derived using either chain 2 or chain 3.Given values for the actuated joints qa, it still remains to solve for the passive

joints qp from the loop constraint equations. Eliminating as many elementsof qp a priori will obviously simplify the task. The second point to note isthat Hp(q) may become singular, in which case qp cannot be obtained fromqa. Configurations in which Hp(q) becomes singular correspond to actuatorsingularities, which are discussed in the next section.


Figure 7.8: Configuration space singularities of the planar five-bar linkage.

7.3 Singularities

In this final section we shall examine the fundamental properties of closed chainsingularities. Characterizing the singularities of closed chains involves manymore subtleties than for open chains. Rather than attempt any such compre-hensive classification for general closed chains, we instead choose to highlightthe essential features of closed chain singularities via two planar examples: afour-bar linkage (see Figure 7.6), and a five-bar linkage (see Figure 7.7). Theexamples should also make clear how our approach to singularity analysis canbe generalized to more complex closed chains.

We begin with the four-bar linkage. Recall that its configuration space is acurve embedded in a four-dimensional ambient space; even without appealingto equations, one can see that the allowable joint values for (θ, φ) of the four-barform a curve of the type shown in Figure 7.6. In terms of the input and outputangles θ and φ, the kinematic loop constraint equations can be expressed as

φ = tan−1 β

α± cos−1

(γ√

α2 + β2

), (7.21)

where

α = 2L3L4 − 2L1L3 cos θ (7.22)β = −2L1L3 sin θ (7.23)γ = L2

2 − L24 − L2

3 − L21 + 2L1L4 cos θ. (7.24)

Obviously the existence and uniqueness of solutions depends on the link lengthsL1, . . . , L4; in particular, a solution fails to exist if γ2 ≤ α2 + β2. The figuredepicts the input-output graph for the choice of link lengths L1 = 4, L2 = 4,L3 = 3, L4 = 2; in this case both θ and φ can range from 0 to 2π.

One of the striking features of this graph is the bifurcation point P asindicated in the figure. Here two branches of the curve meet, resulting in aself-intersection of the curve with itself. If the four-bar is in the configurationindicated by P , it has the choice of following either branch. At no other pointin the four-bar’s joint configuration space does such a phenomenon occur.


Figure 7.9: Actuator singularities of the planar five-bar linkage: the left isnondegenerate, while the right is degenerate.

.

We now turn to the five-bar linkage. The kinematic loop constraint equationscan be written

L1 cos θ1 + . . .+ L4 cos(θ1 + θ2 + θ3 + θ4) = L5 (7.25)L1 sin θ1 + . . .+ L4 sin(θ1 + θ2 + θ3 + θ4) = 0 (7.26)

where we have eliminated joint variable θ5 a priori from the loop closure con-ditions. Writing these two equations in the form f(θ1, . . . , θ4) = 0, wheref : R4 → R2, the configuration space can be regarded as a two-dimensional sur-face in R4. Like the bifurcation point in the four-bar linkage, self-intersectionsof the surface can also occur. At such points the constraint Jacobian loses rank;for the five-bar, any point θ at which

rank(∂f

∂θ(θ))< 2 (7.27)

corresponds to what we call a configuration space singularity. Figure 7.8illustrates the possible configuration space singularities of the five-bar. Noticethat thus far we have made no mention of which joints of the five-bar are ac-tuated, or where the end-effector frame is placed; it is worth emphasizing thatthe notion of configuration space singularity is completely independent of thechoice of actuated joints, or the end-effector frame.

We now consider the case when two joints of the five-bar are actuated. Re-ferring to Figure 7.9, the actuated joints are indicated by filled circles. Undernormal operating conditions, the motions of the actuated joints can be indepen-dently controlled. Alternatively, locking the actuated joints should immobilizethe five-bar and turn it into a rigid structure.

For the nondegenerate actuator singularity shown on the left, rotatingthe two actuated joints in opposite directions will clearly have catastrophicconsequences of the mechanism. For the degenerate actuator singularity


shown on the right, we have the opposite case: even when the actuated jointsare locked in place, the inner two links are free to rotate.

The reason for classifying these singularities as actuator singularities isthat, by relocating the actuators to a different set of joints, such singularities canbe eliminated. For both the degenerate and nondegenerate actuator singularitiesof the five-bar, relocating one of the actuators to one of the three passive jointseliminates the singularity.

Intuitively visualizing the actuator singularities of the planar five-bar isstraightforward enough, but for more complex spaatial closed chains this maybe difficult. Actuator singularities can be characterized mathematically by therank of the constraint Jacobian. As before, write the kinematic loop constraintsin differential form: [

Ha(q) Hp(q)] [ qa

qp

]= 0, (7.28)

where qa ∈ Ra is the vector of actuated joints, and qp ∈ Rp is the vector ofpassive joints. It follows that the matrix

H(q) =[Ha(q) Hp(q)

]∈ Rp×(a+p), (7.29)

and that Hp(q) is a p× p matrix.With the above definitions, we have the following:

• If rank Hp(q) < p, then q is an actuator singularity. Distinguishing be-tween degenerate and nondegenerate singularities involves additionalmathematical subtleties, and relies on second-order derivative informationthat we shall not pursue further here.

• If rank H(q) < p, then q is a configuration space singularity. Notethat under this condition Hp(q) is also singular (the converse is not true,however). The configuration space singularities can thus be regarded asthe intersection of all possible actuator singularities obtained over all pos-sible combinations of actuated joints.

The final class of singularities involves the choice of an end-effector frame.For the five-bar, we ignore the orientation of the end-effector frame, and focusexclusively on its x-y location. Figure 7.10 shows the five-bar in an end-effectorsingularity for the given choice of end-effector location. Note that velocitiesalong the indicated line are not possible in this configuration, similar to the casefor singularities for open chains. Note that end-effector singularities are entirelyindependent of the choice of actuated joints (note that it was not necessary tospecify which, or even how many, of the joints are actuated).

End-effector singularities can be mathematically characterized as follows.Choose any valid set of actuated joints qa such that the mechanism is not at anactuator singularity. Write the forward kinematics in the form

f(qa) = T (7.30)

where T denotes the end-effector frame. One can then check for rank deficienciesin the Jacobian of f , as was done for open chains, to determine the presence ofan end-effector singularity.


Figure 7.10: End-effector singularities of the planar five-bar linkage..

7.4 Summary

• Any kinematic chain that contains one or more loops is called a closedchain. Parallel mechanisms are a class of closed chain that are char-acterized by two platforms—one moving and one stationary—connectedby several legs; the legs are typically open chains, but can themselves beclosed chains. Compared to open chains, the kinematic analysis of closedchains is complicated by the fact that the configuration space is oftencurved, and only a subset of the joints are actuated.

• For a parallel mechanism whose actuated degrees of freedom equals itsmobility, the inverse kinematics problem involves finding, from the givenposition and orientation of the moving platform, the values of all theactuated joints. For well-known parallel mechanisms like the planar 3 ×RPR and spatial Stewart-Gough platform, the inverse kinematics admitsunique solutions.

• For a parallel mechanism whose actuated degrees of freedom equals itsmobility, the forward kinematics problem involves finding, given values forall the actuated joints, the position and orientation of the moving platform.For well-known parallel mechanisms like the 3 × RPR and the spatialStewart-Gough platform, the forward kinematics usually admits multiplesolutions. In the case of the most general Stewart-Gough platform, amaximum of 40 solutions are possible.

• The differential kinematics of a closed chain relates velocities of the actu-ated joints to the linear and angular velocities of the moving platform. Fora closed chain consisting of n one degree of freedom joints, whose actuateddegrees of freedom also equals its mobility m, let θa ∈ Rm denote the vec-tor of actuated joints, and θp ∈ Rn−m denote the vector of passive joints.

7.4. Summary 197

The kinematic loop closure constraints are described by an equation of theform h(θa, θp) = 0, where g : Rn → Rn−m. The forward kinematics can beexpressed in the form f(θa) = T , where f : Rm → SE(3). The differentialkinematics then involves derivatives of both f and g with respect to θaand θp. For platforms like the Stewart-Gough platorm, the differentialkinematics can also be obtained from a static analysis, by exploiting thefact that just as for closed chains, the external forces F at the end-effectorare related to the joint torques τ by τ = JTF .

• Singularities for closed chains can be classifed into three types: (i) configu-ration space singularities occur at, e.g., self-intersections of the configura-tion space surface (or bifurcation points in the event that the configurationspace is a curve); (ii) nondegenerate actuator singularities when the actu-ated joints cannot be independently actuated, while degenerate actuatorsingularities are characterized by the mechanism failing to become a rigidstructure even when all the actuated joints are locked in place; (iii) end-effector singularities occur when the end-effector loses one or more degreesof freedom of mobility. Configuration space singularities are independentof choice of actuated joints, while actuator singularities depend on whichjoints are actuated. End-effector singularities depend on where the end-effector frame is placed, but do not depend on the choice of actuatedjoints.


Figure 7.11: Two cooperating six degree of freedom arms grasping an object.

Figure 7.12: A 3×RPR planar parallel mechanism.

7.5 Exercises

1. Two six degree of freedom arms cooperate to move the disc as shown inFigure 7.11. Given the position and orientation of the disc, how many inversekinematics solutions exist?

2. Consider the 3 × RPR planar parallel mechanism of Figure 7.12, in whichthe prismatic joints are actuated. Define

−−→OAi = ai ∈ R3 with respect to the

7.5. Exercises 199

Figure 7.13: A Delta robot.

fixed frame and−−→PBi = bi ∈ R3 with respect to the moving platform frame.

(a) Solve the inverse kinematics.(b) Derive a procedure to solve the forward kinematics.(c) Is the configuration shown an end-effector singularity? Explain your an-swer by examining the inverse kinematics Jacobian. Is this also an actuatorsingularity?

3. The Delta robot of Figure 7.13 consists of a fixed base connected to a mov-ing platform by three arms. Each arm consists of an upper arm—made up of aspatial parallelogram connected by spherical joints at the ends of each rod—anda lower arm connected to the fixed base by a revolute joint ωi, connected or-thogonally to the upper arm rod.

−−→OAi = ri, ri × ωi = z, AiBi = ai, BiCi = bi,

ωi⊥ai,−−→PCi = hi are defined with respect to the moving platform frame. Derive

step-by-step procedures for solving the following:(a) Solve the forward kinematics.(b) Solve the inverse kinematics.(c) Find the Jacobian Ja.

Chapter 8

Form and Force Closure

Figure 8.1 shows a planar rigid body constrained by a set of four point contacts.The four point contacts can easily be imagined to be the fingertips of a handgrasping an object—for this reason such an arrangement will also be referredto as a grasp. Let us now pose the following question: of the two contactarrangements shown, which is more stable, in the sense of being better able towithstand arbitrary external forces and moments applied to the object?

Clearly whether there is friction at the contacts is relevant (that friction canonly help matters should be fairly obvious), but our intuition tells us that itshould be possible to immobilize the object using only frictionless point contacts,provided we have a sufficient number of them and they are arranged properly.Let us examine this frictionless case more carefully. In arrangement (a), anypure moment applied about the object center would likely cause infinitesimalrotations in the plane, particularly if the contacts were not positioned tightlyagainst the object. Arrangement (b), on the other hand, would seem to bebetter able to withstand arbitrary external forces and moments. If a set offrictionless point contacts completely immobilizes an object with respect toarbitrary external forces and moments, the object is then said to be in formclosure.

Now imagining the contacts to be the fingertips of a hand grasping an object,

Figure 8.1: (a), (b) A planar rigid body constrained by four frictionless pointcontacts. (c) A rigid body constrained by two point contacts with friction.Gravity acts downward.

201

202 Form and Force Closure

if there is no friction at each of the contacts, then intuitively it should be clearthat the fingertips can only apply pushing forces that are normal to the objectsurface at the point of contact. If there were friction, then the fingertips cannow apply forces in non-normal directions, up to a certain angle as measuredfrom the surface normal (the extent of this angle depends on the amount offriction present; the larger the friction coefficient, the greater this angle). Ifthe fingertips are able to generate forces so as to resist any external force ormoment applied to the object, then the object said to be in force closure, andthe grasp is a force closure grasp. Force closure does not necessarily meanthat the object is always stationary. For example, in Figure ??, two fingers withfrictional point contacts are grasping a triangle in the presence of gravity (actingdownward); if, e.g., the motors powering the fingers cannot provide sufficientforces, the triangle may fall despite the grasp being force closure.

In this chapter we shall develop methods for determining whether or not agrasp is form closure (in the case where all contact points are frictionless) orforce closure (in the case when some of the contact points have friction). Onlygrasps that can be modelled as a collection of point contacts will be considered.This is not as restrictive as it may first seem; other types of contacts (e.g.,line and surface contacts) are, for practical purposes, often approximated as acollection of point contacts.

Not surprisingly, the conditions for form and force closure follow directlyfrom the equations for static force-moment equilibrium. As we shall show, theanalysis of form and force closure in the end reduces to a question about theexistence of solutions to a linear equation of the form Ax = b, with a small butimportant twist. The main results of this chapter are a set of computational andgraphical procedures for determining whether a grasp is form closure or forceclosure. After briefly examining contact models, we shall first look at frictionlessgrasps and the form closure conditions, followed by grasps with friction and theforce closure condition.

8.1 Contact Models

We shall assume all objects are rigid bodies, and use Coulomb’s law to modelcontact friction. Suppose a force f is applied at some point as shown in Fig-ure 8.2. If there is no sliding at the contact point, Coulomb’s law states thatthe tangential component of the force, denoted ft, is related to the normalcomponent, denoted fn, by

|ft| ≤ µ|fn|, (8.1)

where µ is called the coefficient of friction. µ is a dimensionless parameterthat depends on the two materials in contact. µ = 0 indicates no friction,while larger values indicate increasing friction. Values of µ for typical materialsgenerally range between 0.1 and 1. Sliding occurs when |ft| > µ|fn|. In thiscase we know from experience that the friction force is directed opposite to thedirection of motion.

8.1. Contact Models 203

Figure 8.2: Tangential and normal components of the applied force. To avoidslipping, the angle β must not exceed α.

Another phenomenon that should be familiar from experience is that oncesliding begins, the resisting frictional force tends to decrease—frictional resis-tance is usually greatest before the occurrence of sliding. To account for thisdifference, two friction coefficients are sometimes used: a static friction coeffi-cient µs when the object is stationary, and a kinetic friction coefficient µk whenthe object is sliding, with µk ≤ µs.

The underlying mechanisms of friction, which are microscopic in nature, arehighly complex and difficult to model. Even more complex friction models havebeen developed that take into account factors such as the speed of sliding, orthe duration of static contact before sliding occurs. In this chapter we shallnot appeal to these complex models, nor even bother to differentiate betweenstatic and kinetic friction. Instead, we shall use a single friction coefficient µ,which turns out to work reasonably well for hard, dry materials that typicalrigid bodies are made of.

It turns out that the no-slip condition |ft| ≤ µ|fn| can be visualized geo-metrically in a convenient way. Referring again to Figure 8.2, to prevent themagnitude of the tangential force ft from exceeding that of the frictional forceµfn, the angle β must not exceed α. In other words, the applied force must lieinside a cone of angle 2α, where α is given by

α = tan−1 µ. (8.2)

Rather than characterize friction by its friction coefficient µ, in this chapter weshall primarily rely on the friction cone characterization. The friction cone fora spatial rigid body is illustrated in Figure 8.3.

With this background, we now describe the three main contact models:

• A frictionless point contact occurs when there is no friction betweenthe fingertip and the object. In this case µ = 0, and forces can only beapplied in the direction of the surface normal.


Figure 8.3: The friction cone in the case of point contact with a spatia rigidbody.

• In the case of a point contact with friction, any contact forces thatlie within the friction cone will not cause any slipping at the contactpoint. Defining the z-axis of the reference frame to be in the directionnormal to the contact surface (see Figure 8.3), and expressing the contactforce in terms of this frame by f = (fx, fy, fz), the friction cone is thencharacterized by {

f ∈ R3 |√f2x + f2

y < |µfz|}. (8.3)

• In the soft-finger contact model, not only can forces be applied withinthe friction cone, but torques can also be applied about the contact normal.This is similar to the situation for human fingers, in which a finite patchof area on our fingertips is in contact with the object, allowing us to applya torque τ about the contact normal. Similar to the Coulomb frictionmodel, a simple model for the frictional moment is given by the torsionalfriction coefficient γ. In this case the friction cone is given by{

f ∈ R3 and τ ∈ R |√f2x + f2

y < µ|fz| and |τ | ≤ γ|fz|}, (8.4)

where f = (fx, fy, fz) is defined in terms of the same reference frame usedearlier. Although we shall not have occasion to use soft finger contactmodels in this chapter, we include its characterization here for complete-ness.

8.2. Frictionless Grasps 205

8.2 Frictionless Grasps

8.2.1 Form Closure and Static Equilibrium

Consider a spatial rigid body that is completely restrained by a set of n friction-less point contacts. If an arbitrary external spatial force is applied to the object,each of the point contacts will exert some reaction force on the object. Becausethe point contacts are frictionless, any reaction forces will only be exerted in thedirection of the object surface normal (it goes without saying that only pushingforces are generated—no pulling is allowed).

To make things more precise, denote the arbitrary external spatial forceapplied to the object, expressed in terms of the fixed reference frame, by Fe =(me, fe), where fe ∈ R3 and me ∈ R3 denote the three-dimensional force andmoment, respectively, expressed in fixed frame coordinates. The rigid body isin form closure if there exists a set of normal contact forces f1, . . . , fn ∈ R3,exerted at their respective point contacts, such that

fe +n∑i=1

fi = 0 (8.5)

me +n∑i=1

ri × fi = 0, (8.6)

for any arbitrary fe and me, where ri ∈ R3 denotes the vector from the referenceframe origin to contact point i. Alternatively, in terms of six-dimensional spatialforces, the two equations above can be expressed more compactly as

Fe + F1 + F2 + . . .+ Fn = 0, (8.7)

where each Fi = (ri × fi, fi), i = 1, . . . , n, denotes the spatial contact forceexerted at contact i. If such a set of contact forces exists, then the object canwithstand any external spatial force, and is in static equilibrium.

8.2.2 A Motivating Planar Example

So far we have managed a mathematical characterization for frictionless forceclosure based on the static force-moment equilibrium equations, but as of yetwe do not have a computational procedure for determining whether a particularfrictionless grasp is form closure. As an intermediate step to developing sucha general procedure, let us attempt to determine if the rectangle in the planarexample of Figure 8.4, which is subject to four frictionless point contacts, is aform closure grasp. Choosing a fixed reference frame as indicated, the compo-nents of each contact reaction force fi ∈ R2, i = 1, . . . , 4, can be written asfollows:

f1 =[

0−x1

], f2 =

[−x2

0

], f3 =

[0x3

], f4 =

[x4

0

], (8.8)


Figure 8.4: A rectangular rigid body constrained by four frictionless point con-tacts.

where each xi ≥ 0, i = 1, 2, 3, 4 are nonnegative to reflect the fact that onlypushing forces are exerted. The resultant force exerted on the object is then

4∑i=1

fi =[x4 − x2

x3 − x1

], (8.9)

while the resultant moment exerted on the object is

4∑i=1

ri × fi =

00

−x1l1

+

00x2l2

+

00x3l3

+

00

−x4l4

. (8.10)

(Note that only the z-component of the resultant moment vector is nonzero;henceforth we shall write the moment equations for planar grasps as a scalarequation.)

Now, in order for the object to be in form closure, the resultant force andmoment should be able to resist any applied external force and moment. Thatis, there should exist nonnegative scalars x1, . . . , x4 ≥ 0 such that the resultantforce (8.9) can cancel any arbitrary external force fe ∈ R2, and the resultantmoment (8.10) can also cancel any arbitrary external moment me ∈ R. Definingb = (−fe,−me) ∈ R3, the conditions for form closure can now be arranged intoa linear equation of the form Ax = b:

0 −1 0 1−1 0 1 0−l1 l2 l3 −l4

x1

x2

x3

x4

=

b1b2b3

. (8.11)


Figure 8.5: A simplified example: (a) For the given b, no nonnegative solutionx ∈ R3 exists. (b) A nonnegative x ∈ R3 exists for any arbitrary b ∈ R2.

If a nonnegative solution x ∈ R4 exists for all b ∈ R3, we can then assert thatthe grasp is form closure.

Without the requirement that x ≥ 0, it is a simple matter to determinewhether a solution to Ax = b exists for all arbitrary b: it is enough to verifythat the columns of A span R3, or equivalently, that A is of maximal rank(three in this case). However, the question of existence of solutions becomesless straightforward when the nonnegativity constraint x ≥ 0 is imposed.

To gain insight into this question of existence of solutions, consider thefollowing lower-dimensional version of the same problem:

[1 0 −10 1 1

] x1

x2

x3

=[b1b2

], (8.12)

where here A ∈ R2×3, x ∈ R3, and b ∈ R2. Denoting the three columns of A bya1, a2, a3 ∈ R2, Equation (8.12) can be rewritten in the form

a1x1 + a2x2 + a3x3 = b. (8.13)

If we take b as shown in Figure 8.5(a), it is clear that no nonnegative solutionx ∈ R3 exists; b falls outside the space spanned by all nonnegative linear combi-nations of the columns {a1, a2, a3} (by nonnegative linear combination we meanthat all the weights xi in Equation (8.13) are nonnegative). If on the other handthe third column a3 were replaced by (−1,−1) (see Figure 8.5(b)), then clearlythe set of all nonnegative linear combinations of {a1, a2, a3} does span all of R2.In this case it is always possible to find, for all b ∈ R2, a nonnegative solutionx ≥ 0 to Ax = b.


y

z

x

f1

f2

f3

f4

(b)(a)

Figure 8.6: (a) A planar rigid body constrained by four symmetric frictionlesspoint contacts. (b) The associated convex hull.

A more geometric way to characterize this condition is in terms of the trianglewhose vertices are given by the columns {a1, a2, a3} (see Figure 8.5). It is notdifficult to see that a nonnegative solution x ∈ R2 exists if and only if the originlies in the interior of the triangle. Note also that it is not enough if the originlies on the boundary of the triangle; it must lie in the interior.

Returning now to our original example involving the rectangular object,from the previous low-dimensional analogy we can now reasonably conjecturethat in order for the grasp of Figure 8.4 to be form closure, nonnegative lin-ear combinations of the columns of A ∈ R3×4 in Equation (8.11) must spanR3. Equivalently, the tetrahedron in R3 whose vertices are given by the fourcolumns of A must completely enclose the origin; that is, the origin must lie inthe interior of the tetrahedron. One immediate and important consequence ofthis characterization is that a minimum of four frictionless contact points arerequired for planar force closure.

Continuing with the rectangular example, if the offsets l1, . . . , l4 are all pos-itive, then with some effort one can draw and visualize the tetrahedron formedby the columns of A, and verify that the origin does indeed lie in its interior. Ifon the other hand l1 = l2 = l3 = l4 = 0, then we have the situation depicted inFigure 8.6; the A matrix in this case becomes

A =

1 0 −1 00 −1 0 10 0 0 0

. (8.14)

In this case the associated tetrahedron collapses to a planar polygon with ver-tices at {(1, 0), (0,−1), (−1, 0), (0, 1)}. Because the tetrahedron is no longerthree-dimensional, the origin cannot be regarded as lying in its interior. Physi-cally, the contacts are unable to resist pure moments applied to the object.


Figure 8.7: The convex hull of various sets of points in R2.

8.2.3 A Convex Hull Test for Form Closure

We saw in the previous section that in the case of planar rigid bodies, a minimumof four frictionless point contacts is needed to have any hope of form closurein the plane. Let us now formulate the form closure conditions for generalspatial rigid bodies, involving an arbitrary number of frictionless point contacts.Answering the question of whether a grasp is form closure ultimately reduces,in the end, to a question of existence of nonnegative solutions x ≥ 0 to a linearequation Ax = b, for arbitrary b. We therefore begin with some general resultsrelating to this question. Specifically, let us consider Ax = b, where x ∈ Rn,A ∈ Rm×n, m < n, and b ∈ Rm. The precise question we wish to answer is,under what conditions on A does a nonnegative solution x exist for any arbitraryb?

The answer is that nonnegative linear combinations of the columns of Amust span Rm. That is, for any arbitrary b ∈ Rm, there must exist nonnegativescalar weights x1, x2, . . . , xn ≥ 0 such that

a1x1 + a2x2 + . . .+ anxn = b, (8.15)

where each ai ∈ Rm, i = 1, . . . , n, denotes the i-th column of A. We shall notattempt any sort of mathematically rigorous proof of this claim, but it shouldbe intuitively clear that the results of our earlier planar motivating examplegeneralize directly to this result. Also, like the planar example, the answer tothis existence question can be characterized geometrically in terms of whetherthe origin lies in the interior of some polyhedron in Rn.

To state this equivalent geometric condition more precisely, we will first needto define the notion of convex hull. Given a set S of n vectors in Rm, i.e.,

S = {a1, . . . , an}, ai ∈ Rm, i = 1, . . . , n, (8.16)

the convex hull of S is the set of convex combinations of {a1, . . . , an}, i.e.,

Convex hull of S =

{n∑i=1

wiai

∣∣∣∣∣n∑i=1

wi = 1, wi ≥ 0 for all i

}. (8.17)

Figure 8.7 illustrates the convex hull of various sets of points in R2. The require-ment that the nonnegative linear combinations of the columns of A span Rm


can now be equivalently stated as follows: there must exist some m-dimensionalopen ball in Rm, centered at the origin, that lies in the interior of the convexhull of the columns of A.

Let us now answer the question originally posed at the beginning of thissection. Consider a rigid body in R3 constrained by n frictionless point contacts.Let ri ∈ R3 denote the vector from the fixed frame origin to contact point i,expressed in fixed frame coordinates, and let ni ∈ R3 denote a vector normal tothe body at contact point i, directed toward the interior of the body. Assumingthe body is in static equilibrium, an arbitrary external force fe ∈ R3 and externalmoment me ∈ R3 applied to the body generates a set of contact forces at eachcontact point. Let fi ∈ R3 denote the contact force at contact i; because thecontact can only generate forces directed toward the interior of the body, eachfi is of the form

fi = nixi, (8.18)

where each xi ≥ 0 is a nonnegative scalar. The static force-equilibrium equa-tions (8.5)-(8.6) can then be written in the form

[n1 · · · nn

r1 × n1 · · · rn × nn

] x1

...xn

=[−fe−me

]. (8.19)

The above equations are of the form Ax = b, with A ∈ R6×n, x ∈ Rn, andb ∈ R6. The grasp is form closure if there exists, for all arbitrary b, a nonnegativex ∈ Rn satisfying Ax = b. Geometrically, the grasp is form closure if and onlyif there exists a six-dimensional open ball centered at the origin that lies in theinterior of the convex hull formed by the columns of A. This is both a necessaryand sufficient condition for form closure, as first shown by Mishra, Schwartz,and Sharir [23]. An immediate consequence of this geometric characterizationis that, for spatial rigid bodies, a minimum of seven frictionless contact points(that is, n = 7) are required to have any hope of form closure.

Clearly it is not an easy matter to visually determine whether an open balllies in the interior of some convex hull for even n = 3, let alone n = 6. In thenext section we shall develop a numerical procedure for doing so.

8.2.4 A Computational Test for Form Closure

As noted earlier, the test for form closure ultimately reduces to a linear algebraicquestion of whether there exists, for every arbitrary b ∈ Rm, a nonnegativesolution x ≥ 0, x ∈ Rn, to the linear equation Ax = b, where A ∈ Rm×n,m ≤ n. In the case where n = m + 1—for example, in the planar rectangleexample considered earlier, n = 3 while the number of contacts m = 4—Gauss-Jordan elimination, a widely used method for finding solutions to the linearequation Ax = b with no constraints imposed on x, is helpful.

We first review Gauss-Jordan elimination by means of an example. Considerthe planar grasp of Figure 8.8, in which an L-shaped object is restrained by three


Figure 8.8: A planar object restrained by three frictionless point contacts.

frictionless point contacts. Fix a reference frame as shown at the interior corner.Note that the contact placed at the interior corner can exert reaction forces inboth the −x and −y directions; denote these reaction forces respectively by f1

and f2, and the reaction forces at the remaining two contacts by f3 and f4. Interms of the chosen reference frame coordinates,

f1 =[−10

]x1, f2 =

[0−1

]x2, f3 =

[10

]x3, f4 =

[01

]x4, (8.20)

where each xi ≥ 0, i = 1, . . . , 4. Let (fext,x, fext,y) ∈ R2 denote an arbitraryexternal force, and mext,z ∈ R an arbitrary external moment. Form closure canthen be determined by examining if there exist nonnegative solutions x ∈ R4 tothe linear equation −1 0 1 0

0 −1 0 10 0 −L L

x1

x2

x3

x4

=

−fext,x−fext,y−mext,z

(8.21)

for arbitrary (fext,x, fext,y,mext,z). By using elementary row operations, theabove equation can be reduced into the following row rechelon form: −1 0 1 0

0 −1 0 10 0 −L L

x1

x2

x3

x4

=

−fext,x−fext,y−mext,z

(8.22)

1 0 −1 00 1 0 10 0 1 L

x1

x2

x3

x4

=

fext,xfext,y

mext,z/L

(8.23)


1 0 0 −10 1 0 −10 0 1 −1

x1

x2

x3

x4

=

fext,x +mext,z/Lfext,y

mext,z/L

. (8.24)

(In Gauss-Jordan elimination, zeros are placed both above and below each pivotin the matrix, whereas Gaussian elimination only places zeros below each pivot,and not above; consult any linear algebra reference for details of these twoalgorithms.) The matrix A has been transformed to reduced row echelon formwhile still preserving the solution; that is, any solution to (8.21) is also a solutionto (8.24), and vice versa. The fourth column of the reduced row echelon formmatrix has all elements negative, and it should be fairly clear that a nonnegativesolution x ≥ 0 will exist for any given set of external forces and moments. Statedanother way, nonnegative linear combinations of the four columns of the reducedrow echelon matrix span R3, and thus the grasp is a form closure grasp.

In the previous example, the matrix A ∈ Rm×n was of dimension m = 3 andn = 4. If n > m+ 1, things get a little more complicated. Suppose A ∈ Rm×n,n > m + 1, is of full rank m, and can be reduced to the following row echelonform via Gauss-Jordan elimination:[

I S], (8.25)

where I ∈ Rm×m is the m×m identity matrix, and S ∈ Rm×(n−m). In this case,form closure holds if there exists a positive linear combination of the columns ofS such that all its entries are negative. That is, if there exists some ω ∈ R(n−m),ω ≥ 0, such that v = Sω has all entries negative, then form closure holds.

For example, if

S =[−2 1

2 −2

], (8.26)

then the feasible region for the inequalities Sω < 0, ω ≥ 0 is shown in Figure 8.9.If on the other hand S is defined as

S =[−1 10

5 −2

], (8.27)

then there is no ω simultaneously satisfying ω ≥ 0 and Sω < 0, so the feasibilityregion does not exist.

For S ∈ Rm×n where m and n are arbitrary, one way to determine formclosure is to formulate it as an optimization problem of the following form:

minω,µ

µ (8.28)

subject to the linear inequality constraints ω ≥ 0 and Sω < µ · 1, where 1 is avector all of whose entries are 1. Because both the objective function (i.e., thefunction being minimized, in our case µ) and the constraints are linear in theoptimization variables (ω, µ), this type of optimization problem is called a linearprogramming problem. The details of the optimization are beyond the scope of


Figure 8.9: The feasibility region corresponding to the inequality given in Equa-tion (8.26).

this book, but numerical solvers are readily available. For the problem at hand,given an initial guess for ω and µ, the optimizer then proceeds to minimize µ.If the final value of µ is greater than or equal to zero, then there is no feasible ωthat satisfies Sω < 0 and ω ≥ 0, and the grasp is not form closure. For practicalreasons one may wish to add a negative lower bound on feasible values of µ soas to terminate the optimization within a reasonable time (the closer this lowerbound for µ is to zero, the faster the algorithm will terminate).

We now have a computational procedure for determining whether a friction-less grasp is form closure:

Computational Algorithm for Spatial Form Closure

• Preliminaries: Given a spatial rigid body constrained by a set of nfrictionless point contacts, where n ≥ 7, choose a fixed reference frame,and let ri ∈ R3 be the vector from the fixed frame origin to contact pointi, i = 1, . . . , n.

• Initialization: Let ni ∈ R3 be the vector normal to the rigid body atcontact i that is directed toward the interior of the body. Construct thefollowing matrix A ∈ R6×n:

A =[

n1 · · · nnr1 × n1 · · · rn × nn

]. (8.29)


Figure 8.10: (a) A planar rigid body constrained by two point contacts withfriction. (b) Force decomposition diagram.

• Form Closure Test: If rank(A) is less than six, then the grasp is notform closure. Otherwise, transform A to the following reduced row-echelonform:

A→[I S

], (8.30)

where I is the 6× 6 identity matrix, and S ∈ R6×(n−6).

• Solve the following linear programming problem:

minω,µ

µ (8.31)

subject to ω > 0 and Sω < µ · 1, where 1 is a vector all of whose entriesare 1. If the optimal value of µ is greater than or equal to zero, then thegrasp is not form closure. If the optimal value of µ is strictly less thanzero, then the grasp is form closure.

8.3 Grasps with Friction

8.3.1 A Motivating Planar Example

We now consider grasps involving point contacts with friction. To develop in-tuition, first consider the rectangular planar object of Figure 8.10(a), whichis constrained at each of the two vertical sides by a single point contact withfriction. Recall from our earlier discussion of contact models that the friction co-efficient at the point of contact can be characterized geometrically by its frictioncone: the interior angle of the friction cone, denoted 2α, is related to the fric-tion coefficient µ by µ = tanα. Let e1, e2 ∈ R2 be vectors whose directions arealigned along the respective two edges of the left friction cone; define e3, e4 ∈ R2

in a similar way for the right friction cone. Attaching a reference frame at thecenter as shown in the figure, the directions for the ei can be written as

e1 =[

1µ

], e2 =

[1−µ

], e3 =

[−1−µ

], e4 =

[−1µ

]. (8.32)

Assuming the contact forces at the two contacts, denoted fa and fb, re-spectively, both lie inside their respective friction cones, fa and fb can then be

8.3. Grasps with Friction 215

written

fa = e1x1 + e2x2 (8.33)fb = e3x3 + e4x4, (8.34)

for x1, . . . , x4 ≥ 0. The force closure condition now follows from the equationsfor static equilibrium: for any arbitrary external force fe ∈ R2 and externalmoment me ∈ R, there must exist contact forces fa and fb such that

fa + fb = −fema +mb = −me.

These equations can also be written explicitly in the form Ax = b: 1 1 −1 −1µ −µ −µ µ−µr µr −µr µr

x1

x2

x3

x4

=[−fe−me

]. (8.35)

Force closure requires that, for any arbitrary b ∈ R3 as given above, there existsome nonnegative x ∈ R4 that satisfies Ax = b.

8.3.2 A Convex Hull Test for Planar Force Closure

By now the reader should have observed that the previously derived convexhull test derived for form closure can also be applied here. More generally,determining whether a grasp is force closure ultimately reduces, in the end, tothe same question of existence of nonegative solutions x ≥ 0 to Ax = b, for anyarbitrary b.

Without further ado, we present the convex hull test for determining forceclosure of planar frictional grasps.

Convex Hull Algorithm for Evaluating Planar Force Closure

• Preliminaries: Given a planar rigid body restrained by n point contactswith friction, fix a reference frame, and let ri = (rix, riy ∈ R2 be the vectordirected from the origin of the fixed frame to contact point i, i = 1, . . . , n.Let e2i−1 = (e(2i−1)x, e(2i−1)y) ∈ R2, e2i = (e2ix, e2iy) ∈ R2 respectivelybe vectors directed along the the two edges of the friction cone at contacti, i = 1, . . . , n.

• Constructing the A Matrix: Define the 3× 2n matrix A as follows:

A =

e1x e2x · · ·e1y e2y · · ·

r1xe1y − r1ye1x r1xe2y − r1ye2x · · ·

· · · e(n−1)x enx· · · e(n−1)y eny· · · rnxe(n−1)y − rnye(n−1)x rnxeny − rnyenx

. (8.36)


f 1

f 2

Figure 8.11: Nguyen’s Theorem for point contacts with friction.

• Convex Hull Test: Determine if the origin lies in the interior of theconvex hull of the columns of A.

For the convex hull test, one can use, e.g., the computational procedure de-scribed earlier.

8.3.3 Nguyen’s Theorem for Planar Force Closure

In the case of planar grasps involving two point contacts with friction, Nguyen’sTheorem [26] provides a particularly simple and appealing test for determiningforce closure (see Figure 8.11):

Theorem 8.1. (V. Nguyen) A planar rigid body constrained by two point con-tacts with friction is in force closure if and only if the line connecting the contactpoints lies inside both friction cones.

Let us now show why Nguyen’s Theorem is true. Referring to Figure 8.12,without loss of generality a reference frame can be chosen such that its origin isthe midpoint of the line between the contact points, with the y-axis overlappingthe line. Again without loss of generality, let the distance between the contactpoints be 2. We now consider the four possible cases shown in Figure 8.12: (i)when the line of contact lies inside both friction cones; (ii) when it lies inside onlyone friction cone; (iii) when it lies outside both friction cones, and the frictioncones are on opposite sides of the line of contact; (iv) when it lies outside bothfriction cones, and both friction cones lie on the same side of the line of contact.

For each of these four cases we apply the planar version of the convex hulltest. In the first case when the line of contact lies inside both friction cones, the


Figure 8.12: An illustration of the proof of Nguyen’s Theorem.

Figure 8.13: A spatial rigid body restrained by three point contacts with friction.

static equilibrium equations are of the form

− sin θ1 sin θ2 − sin θ3 sin θ4

cos θ1 cos θ2 − cos θ3 − cos θ4

− sin θ1 sin θ2 sin θ3 − sin θ4

x1

x2

x3

x4

=

b1b2b3

where θi denotes the angle from the line of contact to the friction cone (seeFigure 8.12). The object is in force closure if, for every b ∈ R3, there existsx ≥ 0 that satisfies (8.3.3). Applying the convex hull test, it can be verifiedthat this case satisfies the force closure conditions. In a similar fashion, it canbe verified that the remaining three cases are not force closure.


Figure 8.14: Three possibilities for the intersection between a friction cone anda plane.

8.3.4 Spatial Force Closure for Rigid Bodies Subject to ThreePoint Contacts with Friction

We now consider force closure for spatial rigid bodies. The first question to askis how many point contacts with friction are required to achieve force closure.We saw that for planar bodies, Nguyen’s Theorem asserts that two are enough,as long as the line between the two contact points lies inside both friction cones.For spatial bodies two is clearly not enough—even if the line between the con-tacts lies inside both friction cones (note that these are now cones in R3), it isnot possible to resist any external moment applied about this line. The nextquestion, then, is whether three point contacts with friction are enough to con-struct a force closure grasp.

The answer is yes; in fact, there is a particularly simple and appealing resultdue to Li et al [16] that reduces the force closure analysis of spatial frictionalgrasps into a planar force closure problem. Referring to Figure 8.13, supposea rigid body is constrained by three point contacts with friction. If the threecontact points happened to be collinear, then obviously any moment appliedabout this line cannot be resisted by the three contacts. We can thereforeexclude this case, and assume that the three contact points are not collinear.The three contacts then define a unique plane S, and at each contact point,three possibilities arise (see Figure 8.14):

• The friction cone intersects S in a planar cone;

• The friction cone intersects S in a line;


• The friction cone intersects S in a point.

Li et al [16] show that the object is in force closure if and only if each of thefriction cones intersects S in a planar cone, and S is also in planar force closure:

Theorem 8.2. Given a spatial rigid body restrained by three point contacts withfriction, assume that the three contact points lie on a unique plane S, and thefriction cone at each of the contacts intersects S in a cone. The body is in forceclosure if and only if the the plane S is in a planar force closure grasp.

We now prove this result. First, the necessity condition—if the spatial rigidbody is in force closure, then each of the friction cones intersects S in a planarcone and S is also in planar force closure—is easily verified: if the body is inspatial force closure, then S (which is a part of the body) must also be in planarforce closure. Moreover, if even one friction cone intersects S in a line or point,then there will be external moments (e.g., about the line between the remainingtwo contact points) that cannot be resisted by the grasp.

To prove the sufficiency condition—if each of the friction cones intersects Sin a planar cone and S is also in planar force closure, then the spatial rigid bodyis in force closure—choose a fixed reference frame such that S lies in the x-yplane, and let ri ∈ R3 denote the vector from the fixed frame origin to contactpoint i (see Figure 8.13). Denoting the contact force at i by fi ∈ R3, the contactspatial force Fi ∈ R6 is then of the form

Fi =[fimi

], (8.37)

where each mi = ri × fi, i = 1, 2, 3. Denote the arbitrary external spatial forceFext ∈ R6 by

Fext =[fextmext

]∈ R6. (8.38)

Force closure then requires that there exist contact spatial forces Fi, i = 1, 2, 3,each lying inside its respective friction cone, such that for any external spatialdisturbance force Fext, the following equality is satisfied:

F1 + F2 + F3 + Fext = 0, (8.39)

or equivalently,

f1 + f2 + f3 + fext = 0 (8.40)(r1 × f1) + (r2 × f2) + (r3 × f3) +mext = 0, (8.41)

If each of the contact forces and moments, as well as the external force and mo-ment, is orthogonally decomposed into components lying on the plane spannedby S (corresponding to the x-y plane in our chosen reference frame) and itsnormal subspace N (corresponding to the z-axis in our chosen reference frame),


then the previous force closure equality relations can be written

f1S + f2S + f3S = −fext,S (8.42)(r1 × f1S) + (r2 × f2S) + (r3 × f3S) = −mext,S (8.43)

f1N + f2N + f3N = −fext,N (8.44)(r1 × f1N ) + (r2 × f2N ) + (r3 × f3N ) = −mext,N . (8.45)

In what follows we shall use S to refer both to the slice of the rigid bodycorresponding to the x-y plane, as well as the x-y plane itself. N will always beidentified with the z-axis.

Proceeding with the proof of sufficiency, we now show that if S is in pla-nar force closure, then the body is in spatial force closure. In terms of Equa-tions (8.44)-(8.45) we wish to show that, for any arbitrary forces fext,S ∈ S,fext,N ∈ N and arbitrary moments mext,S ∈ S, mext,N ∈ N , there exist contactforces fiS ∈ S, fiN ∈ N , i = 1, 2, 3, that satisfy (8.44)-(8.45), and such that foreach i = 1, 2, 3, the contact force fi = fiS + fiN lies in friction cone i.

First consider the force closure equations (8.44)-(8.45) in the normal direc-tion N . Given an arbitrary external force fext,N ∈ N and external momentmext,S ∈ S, Equations (8.44)-(8.45) constitute a set of three linear equations inthree unknowns. From our hypothesis that the three contact points are nevercollinear, these equations will always have a unique solution set {f∗1N , f∗2N , f∗3N}in N .

Since S is assumed to be in planar force closure, for any arbitrary fext,S ∈ Sand mext,N ∈ N , there will exist planar contact forces fiS ∈ S, i = 1, 2, 3, thatlie inside their respective planar friction cones and also satisfy Equations (8.42)-(8.43). This solution set is not unique: one can always find a set of internalforces ηi ∈ S, i = 1, 2, 3, each lying inside its respective friction cone, satisfying

η1 + η2 + η3 = 0 (8.46)(r1 × η1) + (r2 × η2) + (r3 × η3) = 0. (8.47)

(To see why such ηi exist, recall that since S is assumed to be in planar forceclosure, solutions to (8.42)-(8.43) must exist for fext,S = µext,N = 0; thesesolutions are precisely the internal forces ηi). Note that these two equationsconstitute three linear equality constraints involving six variables, so that thereexists a three-dimensional linear subspace of solutions for {η1, η2, η3}.

Now if {f1S , f2S , f3S} satisfy (8.42)-(8.43), then so will {f1S + η1, f2S +η2, f3S + η3}. The internal forces {η1, η2, η3} can in turn be chosen to havesufficiently large magnitude so that the contact forces

f1 = f∗1N + f1S + η1 (8.48)f2 = f∗2N + f2S + η2 (8.49)f3 = f∗3N + f3S + η3 (8.50)

all lie inside their respective friction cone. This completes the proof of thesufficiency condition.

8.4. Summary 221

8.4 Summary

• A rigid body subject to a set of fixed point contacts is called a grasp. Agrasp is said to be form closure if arbitrary external forces and momentsapplied to the body do not cause any motion of the body. A grasp is saidto be force closure if forces can be generated at the contacts such thatthe resultant force can cancel any arbitrary external forces or momentsapplied to the object.

• If the point contact is frictionless, contact forces can only be generatedin the direction normal to the object surface, and only directed towardthe interior of the object (contact forces can only push the object; theycannot pull).

• A point contact with friction is characterized by the Coulomb frictioncoefficient µ; µ = 0 indicates no friction, with increasing values of µ corre-sponding to greater friction. The friction cone at a given point contactindicates the set of all directions in which a force can be applied at thecontact such that there is no slipping. The cone’s central axis is directedalong the surface normal, and the width of the cone is 2α radians, withα ∈ [0, π/2] determined from µ = tanα.

• The convex hull of a set of m points {p1, . . . , pm} in Rn is defined to be

{m∑i=1

wipi

∣∣∣∣∣m∑i=1

wi = 1, each wi ≥ 0}.

• Given a spatial rigid body constrained by a set of n frictionless point con-tacts, determining form closure reduces to a question about the existenceof nonnegative solutions x ∈ Rn to the linear equation Ax = b, for ar-bitrary b ∈ R6, where A ∈ R6×n is specified by the grasp parameters.A solution always exists if the convex hull formed by the columns of Acontains some six-dimensional open ball centered at the origin in R6 (ormore intuitively, the origin lies in the interior of this convex hull). Forplanar rigid bodies, A ∈ R3×n, and planar form closure requires that theconvex hull formed by the columns of A contains some three-dimensionalopen ball centered at the origin in R3 (or more intuitively, the origin liesin the interior of this convex hull).

• For a planar rigid body, a minimum of four frictionless point contactsare required to achieve planar form closure. For a spatial rigid body, aminimum of seven frictionless point contacts are required to achieve spatialform closure. Form closure for frictionless grasps can also be determinedfrom a computational procedure: in the spatial case, reducing the matrixA ∈ R6×n to the following row echelon form[

I S],


where I is the 6×6 identity matrix and S ∈ R6×(n−6), if there exists someω ≥ 0 that satisfies Sω < 0, then the grasp is form closure. In the planarcase, A ∈ R3×n, and S in the reduced row echelon form is 3× (n− 3); thesame conditin on S still applies for planar form closure. Determining theexistence of such a ω can be formulated as an optimization problem of thelinear programming type.

• For planar grasps involving n point contacts with friction, determiningforce closure again leads, in the end, to the question of existence of non-negative solutions x ∈ R2n to the linear equation Ax = b for arbitraryb ∈ R3, where A ∈ R3×2n. The same methods involving, e.g., convex hullsand computational procedures for determining frictionless planar grasps,can also be used for planar grasps with friction.

• In the case of two-finger planar grasps with friction, Nguyen’s Theoremstates that force closure is achieved if and only if the line connecting thetwo contact points lies inside both friction cones.

• In the case of three-finger spatial grasps with friction, a theorem by Li etal [16] provides a set of three necessary and sufficient conditions for spatialforce closure: (i) the three contact points define a unique plane S (in otherwords, they cannot be collinear), and (ii) the friction cone at each of thethree contact points intersects S in a planar cone, and (iii) the plane S(more specifically, the intersection between S and the rigid body) must bein planar force closure.


The existence of form and force closure grasps, and the minimum number offingers necessary to achieve these grasps, have been studied in [23], [12], [19],among others. Proofs of Nguyen’s Theorem for both planar and spatial grasps,as well as algorithms for computing two-finger planar force closure grasps, areprovided in his original paper [26]. [16] offers a more detailed statement andproof of the force closure result for spatial three-finger grasps with friction, aswell as constructive algorithms for both planar and spatial force closure grasps.[37] contains a more detailed survey of the grasping literature, including moreadvanced notions of form and force closure (i.e., second-order form and forceclosure [34], [35]).

Exercises

8.5. Exercises 223

Figure 8.15: A 4× 4 planar square restrained by five frictionless point contacts.

Figure 8.16: A planar disk restrained by three frictionless point contacts.

8.5 Exercises

Exercise 1(a) The planar grasp of Figure 8.15 consists of five frictionless point contacts.The square is of size 4× 4. Show that this grasp is not form closure.(b) The grasp of part (a) can be made form closure by adding one frictionlesspoint contact. Draw all the possible locations for this contact.

Exercise 2Assume all contacts shown in Figure 8.16 are frictionless point contacts. De-


termine whether the grasp is form closure. If it is not, how many additionalfrictionless point contacts are needed to construct a form closure grasp?

(a) (b)

Figure 8.17: A planar rigid body with two square holes.

Exercise 3(a) In Figure 8.17-(a), the planar object with two rectangular holes is restrainedfrom the interior by four frictionless point contacts. Determine whether thisgrasp is form closure.(b) In Figure 8.17-(b), the same planar object with two rectangular holes isnow restrained from the interior by three frictionless point contacts. Determinewhether this grasp is form closure.

Figure 8.18: A planar wedge-like object with a hole.

8.5. Exercises 225

Exercise 4The planar object of Figure 8.18 is restrained by four frictionless point contacts.(a) Determine if this grasp is form closure.(b) Suppose the contacts A, B, C, D are now allowed to slide along the half-circle (without crossing each other). Describe the set of all possible form closuregrasps.

(a) (b)

(c)

Figure 8.19: A planar rigid body restrained by point contacts.

Exercise 5(a) Determine whether the grasp of Figure 8.19-(a) is form closure. Assume allcontacts are frictionless point contacts. If the grasp is not form closure, slidethe position of one of the contacts in order to construct a force closure grasp.(b) Now place two frictionless point contacts at the corners as shown in Fig-ure 8.19-(b). Determine if this grasp is form closure.(c) In the grasp of Figure 8.19-(c), contact A is a point contact with friction


Figure 8.20: A planar rigid body restrained by three point contacts with friction.

(a) (b)

Figure 8.21: A planar triangle constrained by three point contacts.

(its friction cone is 90◦ as indicated in the figure), while contacts B and C arefrictionless point contacts. Determine whether this grasp is force closure.

Exercise 6Determine whether the grasp of Figure 8.20 is force closure. Assume all contactsare point contacts with a friction coefficient µ = 1.

Exercise 7(a) In the planar triangle of Figure 8.21(a), contacts A, B, and C are all fric-tionless point contacts. is this grasp form closure? If not, what type of externalforce or moment would cause the object to slip out of the grasp?

8.5. Exercises 227

Figure 8.22: A planar triangle constrained by three point contacts.

Figure 8.23: An L-shaped planar object restrained by two point contacts withfriction.

(b) In the planar triangle of Figure 8.21-(a), suppose now that contact A is apoint contact with friction coefficient µ = 1, while contacts B and C are fric-tionless point contacts. Determine whether the grasp shown is force closure.(c) Now suppose contact point A can be moved to anywhere on the hypotenuseof the triangle as shown in Figure 8.21(b). Determine the range of all contactpoints A for which the grasp is force closure.(d) Contact points B and C are now moved as shown in Figure 8.22. Bill, aclever student, argues that the two contacts B and C can be replaced by avirtual point contact with friction (point D) with the given friction cone, andNguyen’s condition for force closure can now be applied to A and D, as shownin the right Figure 8.22. Is Bill correct? Justify your answer with a derivationof the appropriate force closure condition.

Exercise 8Consider the L-shaped planar object of Figure 8.23.(a) Suppose both contacts are point contacts with friction coefficient µ = 1.


Determine whether this grasp is force closure.(b) Now suppose point contact 1 is a point contact with friction coefficientµ = 1, while point contact 2 is frictionless. Determine whether this grasp isforce closure.(c) Suppose now that the vertical position of contact 1 is allowed to vary; denoteits height by x. Find all positions x such that the grasp of part (b) is forceclosure.

Figure 8.24: A square restrained by three point contacts.

Exercise 9A square is restrained by three point contacts as shown in Figure 8.24: contactf1 is a point contact with friction coefficient µ, while contacts f2 and f3 arefrictionless point contacts. If c = 1

4 and h = 12 , find the range of values of µ

such that grasp is force closure.

Exercise 10In the grasp of Figure 8.25, contacts f1 and f2 on the left are frictionless pointcontacts, while contact f3 on the right is a point contact with friction coefficientµ = 0.2. Determine whether this grasp is force closure.

Exercise 11A single point contact with friction coefficient µ = 1 is applied to the left sideof the square doughnut as shown in Figure 8.26. A force closure grasp can beconstructed by adding another point contact with friction, also with µ = 1.Draw all possible locations for this additional point contact.

8.5. Exercises 229

Figure 8.25: A rectangle restrained by three point contacts.

Figure 8.26: A square doughnut.

Bibliography

[1] J. Angeles, Fundamentals of Robotic Mechanical Systems: Theory, Meth-ods, and Algorithms. Springer, 2006.

[2] R. S. Ball, A Treatise on the Theory of Screws. Cambridge University Press,1998.

[3] O. Bottema and B. Roth, Theoretical Kinematics. Dover Publications,1990.

[4] R. W. Brockett, “Robotic manipulators and the product of exponentialsformula,” Proc. Int. Symp. Math. Theory of Networks and Systems, BeerSheba, Israel, 1983.

[5] H. Choset, K. M. Lynch, S. Hutchinson, G. Kantor, W. Burgard,L. E. Kavraki, S. Thrun, Principles of Robot Motion: Theory, Algorithms,and Implementations. Cambridge, MA: MIT Press, 2005.

[6] J. Craig, Introduction to Robotics: Mechanics and Control. Upper SaddleRiver, New Jersey: Prentice-Hall, 2004.

[7] J. Denavit and R.S. Hartenberg, “A kinematic notation for lower-pair mech-anisms based on matrices,” Trans. ASME J. Appl. Mech, vol. 23, pp. 215-221, 1955.

[8] M. Do Carmo, Differential Geometry of Curves and Surfaces. Upper SaddleRiver, New Jersey: Prentice-Hall, 1976.

[9] A. G. Erdman and G. N. Sandor, Advanced Mechanism Design: Analysisand Synthesis Vol. I and II. Upper Saddle River, New Jersey: Prentice-Hall,1996.

[10] R. Featherstone, Rigid Body Dynamics Algorithms. Upper Saddle River,New Jersey: Springer, 2008.

[11] D. T. Greenwood, Advanced Dynamics. Cambridge: Cambridge UniversityPress, 2003.

231

232 BIBLIOGRAPHY

[12] J. Hong, G. Lafferiere, B. Mishra, and X. Tan, “Fine manipulation withmultifinger hands,” Proc. IEEE Int. Conf. Robotics Autom., pp. 1568-1573,1990.

[13] O. Khatib, “A unified approach to motion and force control of robot manip-ulators: the operational space formulation,” IEEE Int. J. Robotics Autom.,vol. 3, no. 1, pp. 43-53, 1987.

[14] S. M. LaValle, Planning Algorithms. Cambridge: Cambridge UniversityPress, 2006.

[15] H.Y. Lee and C.G. Liang, “A new vector theory for the analysis of spatialmechanisms,” Mech. Machine Theory, vol. 23, no. 3, pp. 209-217, 1988.

[16] J.-W. Li, H. Liu, and H.-G. Cai, “On computing three-finger force-closuregraps of 2-D and 3-D objects,” IEEE Trans. Robotics Autom., vol. 19, no. 1,2003, pp. 155-161.

[17] K. M. Lynch and M. T. Mason, “Pulling by pushing, slip with infinitefriction, and perfectly rough surfaces,” Int. J. Robotics Res., vol. 14, no. 2,pp. 174-183, 1995.

[18] D. Manocha and J. Canny, “Real time inverse kinematics for general manip-ulators,” Proc. IEEE Int. Conf. Robotics Automation, vol. 1, pp. 383-389,1989.

[19] X. Markenscoff, L. Ni, and C. H. Papadimitriou, “The geometry of grasp-ing,” Int. J. Robotics Res., vol. 9, no. 1, pp. 61-74, 1990.

[20] J. M. McCarthy, Introduction to Theoretical Kinematics. Cambridge: MITPress, 1990.

[21] J. P. Merlet, Parallel Robots. Berlin: Springer, 2006.

[22] R. S. Millman and G. D. Parker, Elements of Differential Geometry. UpperSaddle River, New Jersey: Prentice-Hall, 1977.

[23] B. Mishra, J. T. Schwartz, and M. Sharir, “On the existence and synthesisof multifinger positive grips,” Algorithmica, vol. 2, no. 4, pp. 541-558, 1987.

[24] M. T. Mason, Mechanics of Robot Manipulation. Cambridge: MIT Press,2001.

[25] R. Murray, Z. Li, and S. Sastry, A Mathematical Introduction to RoboticManipulation. Boca Raton, Florida: CRC Press, 1994.

[26] V.-D. Nguyen, “Constructing force-closure grasps,” Int. J. Robotics Re-search, vol. 7, no. 3, pp. 3-16, 1988.

[27] F.C. Park, “Computational aspects of the product of exponentials formulafor robot kinematics,” IEEE Trans. Automatic Control, Vol. 39, No. 3,pp. 643-647, 1994.

BIBLIOGRAPHY 233

[28] F.C. Park, J.E. Bobrow, and S.R. Ploen, “A Lie group formulation ofrobot dynamics,” International Journal of Robotics Research, vol. 14, no. 6,pp. 609-618, 1995.

[29] F. C. Park, Jihyeon Choi, and S. R. Ploen, “Symbolic formulation of closedchain dynamics in independent coordinates,” Journal of Mechanism andMachine Theory, vol. 34, no. 5, pp. 731-751, July 1999.

[30] F. C. Park and Jinwook Kim, “Manipulability of closed kinematic chains,”ASME J. Mechanical Design, vol. 120, no. 4, pp. 542-548, 1998.

[31] F. C. Park and Jinwook Kim, “Singularity analysis of closed kinematicchains,” ASME J. Mechanical Design, vol. 121, no. 1, pp. 32-38, 1999.

[32] F. C. Park and I. G. Kang, “Cubic spline algorithms for orientation inter-polation,” Int. .J. Numerical Methods in Engineering, vol. 46, pp. 45-64,1999.

[33] M. Raghavan and B. Roth, “Kinematic analysis of the 6R manipulator ofgeneral geometry,” 5th Int. Symp. Robotics Research, 1990.

[34] E. Rimon and J. Burdick, “Towards planning with force constraints I. Onthe 1st order mobility of bodies in contact,” Mechanisms and MachineTheory, 1995.

[35] E. Rimon and J. Burdick, “Towards planning with force constraints I. Onthe 2nd order mobility of bodies in contact,” Mechanisms and MachineTheory, 1995.

[36] T. Shamir and Y. Yomdin, “Repeatability of redundant manipulators:mathematical solution to the problem,” IEEE Trans. Automatic Control,vol. 33, no. 11, pp. 1004-1009.

[37] B. Siciliano and O. Khatib, Eds., Springer Handbook of Robotics. Berlin-Heidelberg: Springer, 2008.

[38] E.T. Whittaker, A Treatise on the Analytical Dynamics of Particles andRigid Bodies. Cambridge, Cambridge University Press, 1917.

234 BIBLIOGRAPHY

Index

configuration, 11configuration space, 12

degrees of freedom, 11

five-bar linkage,linkagefive-bar, 18

four-bar linkage, 17

Grubler’s formulaplanar linkages, 17spatial linkages, 17

jointcylindrical, 15helical, 15prismatic, 15revolute, 15screw, 15spherical, 15universal, 15

linkagefour-bar, 17

mobility, 11

slider-crank, 18

235

236 INDEX

바로 내려받기 - robotics@snutranslate this...

Documents