当前位置:首页 >> 机械/仪表 >>

Robotics TOOLBOX(机器人工具箱)


Robotics
for MATLAB

TOOLBOX
(Release 7.1)

y

z x

5.5 5 4.5 4

Puma 560

I11

3.5 3 2.5 2 4 2 0 2 !2 0 !2 !4

!4 q2 4

0.8 0.6

q3

Peter I. Corke
Peter.Corke@csiro.au http://www.cat.csiro.au/cmst/staff/pic/robot

April 2002

Peter I. Corke CSIRO Manufacturing Science and Technology Pullenvale, AUSTRALIA, 4069. 2002 http://www.cat.csiro.au/cmst/staff/pic/robot

c 2002 by Peter I. Corke.

3

1
Preface
1 Introduction
This, the seventh release of the Toolbox, represents nearly a decade of tinkering and a substantial level of maturity. It nally includes many of the features I’ve been planning to add for some time, particularly MEX les, Simulink support and modied Denavit-Hartenberg support. The previous release has had thousands of downloads and the mailing list has hundreds of subscribers. The Toolbox provides many functions that are useful in robotics including such things as kinematics, dynamics, and trajectory generation. The Toolbox is useful for simulation as well as analyzing results from experiments with real robots. The Toolbox is based on a very general method of representing the kinematics and dynamics of serial-link manipulators. These parameters are encapsulated in Matlab objects. Robot objects can be created by the user for any serial-link manipulator and a number of examples are provided for well know robots such as the Puma 560 and the Stanford arm. The Toolbox also provides functions for manipulating and converting between datatypes such as vectors, homogeneous transformations and unit-quaternions which are necessary to represent 3-dimensional position and orientation. The routines are written in a straightforward manner which allows for easy understanding, perhaps at the expense of computational efciency. My guide in all of this work has been the book of Paul[1], now out of print, but which I grew up with. If you feel strongly about computational efciency then you can always rewrite the function to be more efcient, compile the M-le using the Matlab compiler, or create a MEX version.

1.1 What’s new
This release has some signicant new functionality as well as some bug xes. Full support for modied (Craig’s) Denavit-Hartenberg notation, forward and inverse kinematics, Jacobians and forward and inverse dynamics. Simulink blockset library and demonstrations included, see Section 2 MEX implementation of recursive Newton-Euler algorithm written in portable C. Speed improvements of at least 1000. Tested on Solaris, Linux and Windows. See Section 1.9. Fixed still more bugs and missing les in quaternion code. Remove ‘@’ notation from fdyn to allow operation under Matlab 5 and 6. Fairly major update of documentation to ensure consistency between code, online help and this manual.

1 INTRODUCTION

4

All code is now under CVS control which should eliminate many of the versioning problems I had previously due to developing the code across multiple computers.

1.2 Contact
The Toolbox home page is at http://www.cat.csiro.au/cmst/staff/pic/robot This page will always list the current released version number as well as bug xes and new code in between major releases. A Mailing List is also available, subscriptions details are available off that web page.

1.3 How to obtain the Toolbox
The Robotics Toolbox is freely available from the Toolbox home page at http://www.cat.csiro.au/cmst/staff/pic/robot The les are available in either gzipped tar format (.gz) or zip format (.zip). The web page requests some information from you regarding such as your country, type of organization and application. This is just a means for me to gauge interest and to help convince my bosses (and myself) that this is a worthwhile activity. The le robot.pdf is a comprehensive manual with a tutorial introduction and details of each Toolbox function. A menu-driven demonstration can be invoked by the function rtdemo.

1.4 MATLAB version issues
The Toolbox works with MATLAB version 6 and greater and has been tested on a Sun with version 6. The Toolbox does not function under MATLAB v3.x or v4.x since those versions do not support objects. An older version of the Toolbox, available from the Matlab4 ftp site is workable but lacks some features of this current Toolbox release.

1.5 Acknowledgements
I am grateful for the support of my employer, CSIRO, for supporting me in this activity and providing me with the Matlab tools and web server. I have corresponded with a great many people via email since the rst release of this Toolbox. Some have identied bugs and shortcomings in the documentation, and even better, some have provided bug xes and even new modules, thankyou.

1 INTRODUCTION

5

1.6 Support, use in teaching, bug xes, etc.
I’m always happy to correspond with people who have found genuine bugs or deciencies in the Toolbox, or who have suggestions about ways to improve its functionality. However I draw the line at providing help for people with their assignments and homework! Many people are using the Toolbox for teaching and this is something that I would encourage. If you plan to duplicate the documentation for class use then every copy must include the front page. If you want to cite the Toolbox please use @ARTICLE{Corke96b, AUTHOR JOURNAL MONTH NUMBER PAGES TITLE VOLUME YEAR }

= = = = = = = =

{P.I. Corke}, {IEEE Robotics and Automation Magazine}, mar, {1}, {24-32}, {A Robotics Toolbox for {MATLAB}}, {3}, {1996}

which is also given in electronic form in the README le.

1.7 A note on kinematic conventions
Many people are not aware that there are two quite different forms of Denavit-Hartenberg representation for serial-link manipulator kinematics: 1. Classical as per the original 1955 paper of Denavit and Hartenberg, and used in textbooks such as by Paul[1], Fu etal[2], or Spong and Vidyasagar[3]. 2. Modied form as introduced by Craig[4] in his text book. Both notations represent a joint as 2 translations (A and D) and 2 rotation angles (! and "). However the expressions for the link transform matrices are quite different. In short, you must know which kinematic convention your Denavit-Hartenberg parameters conform to. Unfortunately many sources in the literature do not specify this crucial piece of information. Most textbooks cover only one and do not even allude to the existence of the other. These issues are discussed further in Section 3. The Toolbox has full support for both the classical and modied conventions.

1.8 Creating a new robot denition
Let’s take a simple example like the two-link planar manipulator from Spong & Vidyasagar[3] (Figure 3-6, p73) which has the following (standard) Denavit-Hartenberg link parameters

1 INTRODUCTION

6

Link 1 2

ai 1 1

!i 0 0

di 0 0

"i " 1 " 2

where we have set the link lengths to 1. Now we can create a pair of link objects: >> L1=link([0 1 0 0 0], ’standard’) L1 = 0.000000 1.000000 0.000000 0.000000 R (std)

>> L2=link([0 1 0 0 0], ’standard’) L2 = 0.000000 1.000000 0.000000 0.000000 R (std)

>> r=robot({L1 L2}) r = noname (2 axis, RR) grav = [0.00 0.00 9.81] alpha A theta 0.000000 1.000000 0.000000 0.000000 1.000000 0.000000 >> The rst few lines create link objects, one per robot link. Note the second argument to link which species that the standard D&H conventions are to be used (this is actually the default). The arguments to the link object can be found from >> help link . . LINK([alpha A theta D sigma], CONVENTION) . . which shows the order in which the link parameters must be passed (which is different to the column order of the table above). The fth element of the rst argument, sigma, is a ag that indicates whether the joint is revolute (sigma is zero) or primsmatic (sigma is non zero).

standard D&H parameters D R/P 0.000000 R (std) 0.000000 R (std)

1 INTRODUCTION

7

2 1.5 1 0.5 yz x

Z

0 !0.5 !1 !1.5 !2 2 1 0 !1 Y !2 !2 0 !1 X 1 2 noname

Figure 1: Simple two-link manipulator model. The link objects are passed as a cell array to the robot() function which creates a robot object which is in turn passed to many of the other Toolbox functions. Note that displays of link data include the kinematic convention in brackets on the far right.
(std) for standard form, and (mod) for modied form.

The robot just created can be displayed graphically by >> plot(r, [0 0]) which will create the plot shown in Figure 1.

1.9 Using MEX les
The Robotics Toolbox Release 7 includes portable C source code to generate a MEX le version of the rne function. The MEX le runs upto 500 times faster than the interpretted version rne.m and this is critical for calculations involving forward dynamics. The forward dynamics requires the calculation of the manipulator inertia matrix at each integration time step. The Toolbox uses a computationally simple but inefcient method that requires evaluating the rne function n + 1 times, where n is the number of robot axes. For forward dynamics the rne is the bottleneck. The Toolbox stores all robot kinematic and inertial parameters in a robot object, but accessing these parameters from a C language MEX le is somewhat cumbersome and must be done on each call. Therefore the speed advantage increases with the number of rows in the q, qd and qdd matrices that are provided. In other words it is better to call rne with a trajectory, than for each point on a trajectory. To build the MEX le: 1. Change directory to the mex subdirectory of the Robotics Toolbox.

1 INTRODUCTION

8

2. On a Unix system just type make. For other platforms follow the Mathworks guidelines. You need to compile and link three les with a command something like mex frne.c ne.c vmath.c. 3. If successful you now have a le called frne.ext where ext is the le extension and depends on the architecture (mexsol for Solaris, mexlx for Linux). 4. From within Matlab cd into this same directory and run the test script
>> cd ROBOTDIR/mex >> check *************************************************************** ************************ Puma 560 ***************************** *************************************************************** ************************ normal case ***************************** DH: Fast RNE: (c) Peter Corke 2002 Speedup is 17, worst case error is 0.000000 MDH: Speedup is 1565, worst case error is 0.000000 ************************ no gravity ***************************** DH: Speedup is 1501, worst case error is 0.000000 MDH: Speedup is 1509, worst case error is 0.000000 ************************ ext force ***************************** DH: Speedup is 1497, worst case error is 0.000000 MDH: Speedup is 637, worst case error is 0.000000 *************************************************************** ********************** Stanford arm *************************** *************************************************************** ************************ normal case ***************************** DH: Speedup is 1490, worst case error is 0.000000 MDH: Speedup is 1519, worst case error is 0.000000 ************************ no gravity ***************************** DH: Speedup is 1471, worst case error is 0.000000 MDH: Speedup is 1450, worst case error is 0.000000 ************************ ext force ***************************** DH: Speedup is 417, worst case error is 0.000000 MDH: Speedup is 1458, worst case error is 0.000000 >>

This will run the M-le and MEX-le versions of the rne function for various robot models and options with various options. For each case it should report a speedup greater than one, and an error of zero. The results shown above are for a Sparc Ultra 10. 5. Copy the MEX-le frne.ext into the Robotics Toolbox main directory with the name rne.ext. Thus all future references to rne will now invoke the MEX-le instead of the M-le. The rst time you run the MEX-le in any Matlab session it will print a one-line identication message.

9

2
Using the Toolbox with Simulink
2 Introduction
Simulink is the block diagram editing and simulation environment for Matlab. Until its most recent release Simulink has not been able to handle matrix valued signals, and that has made its application to robotics somewhat clumsy. This shortcoming has been rectied with Simulink Release 4. Robot Toolbox Release 7 and higher includes a library of blocks for use in constructing robot kinematic and dynamic models. To use this new feature it is neccessary to include the Toolbox Simulink block directory in your Matlab path: >> addpath ROBOTDIR/simulink To bring up the block library >> roblocks which will create a display like that shown in Figure 2. Users with no previous Simulink experience are advised to read the relevant Mathworks manuals and experiment with the examples supplied. Experienced Simulink users should nd the use of the Robotics blocks quite straightforward. Generally there is a one-to-one correspondence between Simulink blocks and Toolbox functions. Several demonstrations have been included with the Toolbox in order to illustrate common topics in robot control and demonstrate Toolbox Simulink usage. These could be considered as starting points for your own work, just select the model closest to what you want and start changing it. Details of the blocks can be found using the File/ShowBrowser option on the block library window.

Robotics Toolbox for Matlab (release 7)
Dynamics
noname tau q qd qdd Robot noname

Graphics

Trajectory
q qd qdd q

Kinematics
noname n J jacobn 0 J noname jacob0 J
x y z

Transform conversion
x T T y z

plot jtraj

xyz2T
roll pitch T

T2xyz
roll T pitch yaw

q qd qdd

noname tau

q TODO

J

J

J

!1 Ji

yaw

rpy2T
a

T2rpy
a T T b c

ijacob T1 dx T2

b c

rne q

noname T

eul2T

T2eul

Copyright (c) 2002 Peter Corke

fkine

tr2diff

Figure 2: The Robotics Toolbox blockset.

3 EXAMPLES

10

Puma560 collapsing under gravity
Puma 560 [0 0 0 0 0 0]’ Zero torque Robot 0 Clock Simple dynamics demo pic 11!Feb!2002 14:19:49 simout To Workspace tau q qd plot qdd Puma 560

Figure 3: Robotics Toolbox example demo1, Puma robot collapsing under gravity.

3 Examples
3.1 Dynamic simulation of Puma 560 robot collapsing under gravity
The Simulink model, demo1, is shown in Figure 3, and the two blocks in this model would be familiar to Toolbox users. The Robot block is similar to the fdyn() function and represents the forward dynamics of the robot, and the plot block represents the plot function. Note the parameters of the Robot block contain the robot object to be simulated and the initial joint angles. The plot block has one parameter which is the robot object to be displayed graphically and should be consistent with the robot being simulated. Display options are taken from the plotbotopt.m le, see help for robot/plot for details. To run this demo rst create a robot object in the workspace,typically by using the puma560 command, then start the simulation using Simulation/Start option from the model toolbar. >> puma560 >> demo1

3.2 Dynamic simulation of a simple robot with exible transmission
The Simulink model, demo2, is shown in Figure 4, and represents a simple 2-link robot with exible or compliant transmission. The rst joint receives a step position demand change at time 1s. The resulting oscillation and dynamic coupling between the two joints can be seen clearly. Note that the drive model comprises spring plus damper, and that the joint position control loops are simply unity negative feedback. To run this demo rst create a 2-link robot object in the workspace,typically by using the
twolink command, then start the simulation using Simulation/Start option from the model

toolbar. >> twolink >> demo2

3 EXAMPLES

11

2!link robot with flexible transmission
load position Puma 560 Step 0 Constant assume the motor is infinitely "stiff" Rate Limiter
motor position

q qd

Puma 560

100 K du/dt Derivative 20 B

tau

plot qdd Robot Scope 0 Clock simout To Workspace

2!link demo pic Mon Apr 8 11:37:04 2002

transmission comprises spring + damper

Figure 4: Robotics Toolbox example demo2, simple exible 2-link manipulator.

3.3 Computed torque control
The Simulink model, demo3, shown in Figure 5, is for a Puma560 with a computed torque control structure. This is a “classical” dynamic control technique in which the rigid-body dynamic model is inverted to compute the demand torque for the robot based on current joint angles and joint angle rates and demand joint angle acceleration. This model introduces the rne block which computes the inverse dynamics using the recursive Newton-Euler algorithm (see rne function), and the jtraj block which computes a vector quintic polynomial. jtraj has parameters which include the initial and nal values of the each output element as well as the overall motion time. Initial and nal velocity are assumed to be zero. In practice of course the dynamic model of the robot is not exactly known, we can only invert our best estimate of the rigid-body dynamics. In the simulation we can model this by using the perturb function to alter the parameters of the dynamic model used in the rne block — note the ’P/’ prex on the model name displayed by that block. This means that the inverse dynamics are computed for a slightly different dynamic model to the robot under control and shows the effect of model error on control performance. To run this demo rst create a robot object in the workspace,typically by using the puma560 command, then start the simulation using Simulation/Start option from the model toolbar.
Puma 560 computed torque control

100 Kp 1 Kd

q qd qdd jtraj trajectory (demand)

q qd qdd

P/Puma 560 tau

Puma 560 tau

q qd qdd robot state (actual)

Puma 560

plot

rne

Robot error

0 Puma560 computed torque control pic 11!Feb!2002 14:18:39 Clock simout To Workspace

Figure 5: Robotics Toolbox example demo3, computed torque control.

3 EXAMPLES

12

>> puma560 >> demo3

3.4 Torque feedforward control
The Simulink model demo4 demonstrates torque feedforward control, another “classical” dynamic control technique in which the demanded torque is computed using the rne block and added to the error torque computed from position and velocity error. It is instructive to compare the structure of this model with demo3. The inverse dynamics are not in the forward path and since the robot conguration changes relatively slowly, they can be computed at a low rate (this is illustrated by the zero-order hold block sampling at 20Hz). To run this demo rst create a robot object in the workspace,typically by using the puma560 command, then start the simulation using Simulation/Start option from the model toolbar. >> puma560 >> demo4

3.5 Cartesian space control
The Simulink model, demo5, shown in Figure 6, demonstrates Cartesian space motion control. There are two conventional approaches to this. Firstly, resolve the Cartesian space demand to joint space using inverse kinematics and then perform the control in joint space. The second, used here, is to compute the error in Cartesian space and resolve that to joint space via the inverse Jacobian. This eliminates the need for inverse kinematics within the control loop, and its attendent problems of multiple solutions. It also illustrates some additional Simulink blocks. This demonstration is for a Puma 560 robot moving the tool in a circle of radius 0.05m centered at the point (0.5, 0, 0). The difference between the Cartesian demand and the current Cartesian pose (in end-effector coordinates) is computed by the tr2diff block which produces a differential motion described by a 6-vector. The Jacobian block has as its input the current manipulator joint angles and outputs the Jacobian matrix. Since the differential motion is with respect to the end-effector we use the JacobianN block rather than Jacobian0. We use standard Simulink block to invert the Jacobian and multiply it by
Cartesian control
q

Bad Link

J

J

!1 Ji

Cartesian circle 0 Clock 0.05*sin(u) f(u)
x y z

jacob0

ijacob Puma 560

Puma 560

plot

T

T1 dx T2 tr2diff

Matrix Multiply

!0.6

1 s Rate controlled robot axes

q

q

T

xyz2T

fkine
T

x y z

XY Graph

T2xyz 0

Figure 6: Robotics Toolbox example demo5, Cartesian space control.

3 EXAMPLES

13

the differential motion. The result, after application of a simple proportional gain, is the joint space motion required to correct the Cartesian error. The robot is modelled by an integrator as a simple rate control device, or velocity servo. This example also demonstrates the use of the fkine block for forward kinematics and the T2xyz block which extracts the translational part of the robot’s Cartesian state for plotting on the XY plane. This demonstration is very similar to the numerical method used to solve the inverse kinematics problem in ikine. To run this demo rst create a robot object in the workspace,typically by using the puma560 command, then start the simulation using Simulation/Start option from the model toolbar. >> puma560 >> demo5

3.6 Image-based visual servoing
The Simulink model, demo6, shown in Figure 7, demonstrates image-based visual servoing (IBVS)[5]. This is quite a complex example that simulates not only the robot but also a camera and the IBVS algorithm. The camera is assumed to be mounted on the robot’s endeffector and this coordinate is passed into the camera block so that the relative position of the target with respect to the camera can be computed. Arguments to the camera block include the 3D coordinates of the target points. The output of the camera is the 2D image plane coordinates of the target points. The target points are used to compute an image Jacobian matrix which is inverted and multiplies the desired motion of the target points on the image plane. The desired motion is simply the difference between the observed target points and the desired point positions. The result is a velocity screw which drives the robot to the desired pose with respect to a square target. When the simulation starts a new window, the camera view, pops up. We see that initially the square target is off to one side and somewhat oblique. The image plane errors are mapped by an image Jacobian into desired Cartesian rates, and these are futher mapped by a
Image!based visual servo control
q 6

Puma 560 MATLAB Function 146.1 visjac condition

plot
[6x6]

MATLAB Function cond()

6.998 manip jac condition

[8x6]

Puma 560
6

q

n J jacobn

J

[6x6] [6x6]

J

J

!1 Ji

[6x6]

Puma 560 Matrix 6 Multiply 1 s Rate controlled robot axes
6 q 6

q

T

[4x4]

T

uv

[4x2] [4x2]

uv

J

[8x6] [8x6]

MATLAB Function pinv

[6x8]

ijacob

6

Matrix 6 Multiply
8

fkine camera 256 456 456 256 0.21 0.28 !0.32 !0.00 0.04 !0.01
6

visual Jacobian
[4x2] [4x2] [4x2]

456 456 256 256

MATLAB Function

8 feature vel

desired image plane coordinates

85.07 107.89 112.87 109.40 108.90 80.92 81.10 80.81
8 8 8

MATLAB Function

274 feature error norm

Cartesian velocity dmd Image!based visual servo control pic 08!Apr!2002 11:31:20
6

feature error

feature error !0.10
6

desired camera velocity

Figure 7: Robotics Toolbox example demo6, image-based visual servoing.

3 EXAMPLES

14

manipulator Jacobian into joint rates which are applied to the robot which is again modelled as a rate control device. This closed-loop system is performing a Cartesian positioning task using information from a camera rather than encoders and a kinematic model (the Jacobian is a weak kinematic model). Image-based visual servoing schemes have been found to be extremely robust with respect to errors in the camera model and manipulator Jacobian.

15

3
Tutorial
3 Manipulator kinematics
Kinematics is the study of motion without regard to the forces which cause it. Within kinematics one studies the position, velocity and acceleration, and all higher order derivatives of the position variables. The kinematics of manipulators involves the study of the geometric and time based properties of the motion, and in particular how the various links move with respect to one another and with time. Typical robots are serial-link manipulators comprising a set of bodies, called links, in a chain, connected by joints 1 . Each joint has one degree of freedom, either translational or rotational. For a manipulator with n joints numbered from 1 to n, there are n + 1 links, numbered from 0 to n. Link 0 is the base of the manipulator, generally xed, and link n carries the end-effector. Joint i connects links i and i 1. A link may be considered as a rigid body dening the relationship between two neighbouring joint axes. A link can be specied by two numbers, the link length and link twist, which dene the relative location of the two axes in space. The link parameters for the rst and last links are meaningless, but are arbitrarily chosen to be 0. Joints may be described by two parameters. The link offset is the distance from one link to the next along the axis of the joint. The joint angle is the rotation of one link with respect to the next about the joint axis. To facilitate describing the location of each link we afx a coordinate frame to it — frame i is attached to link i. Denavit and Hartenberg[6] proposed a matrix method of systematically assigning coordinate systems to each link of an articulated chain. The axis of revolute joint i is aligned with zi1 . The xi1 axis is directed along the normal from z i1 to zi and for intersecting axes is parallel to z i1 × zi . The link and joint parameters may be summarized as: link length link twist link offset joint angle ai !i di "i the offset distance between the z i1 and zi axes along the xi axis; the angle from the z i1 axis to the zi axis about the x i axis; the distance from the origin of frame i 1 to the x i axis along the zi1 axis; the angle between the x i1 and xi axes about the zi1 axis.

For a revolute axis " i is the joint variable and d i is constant, while for a prismatic joint d i is variable, and "i is constant. In many of the formulations that follow we use generalized coordinates, q i , where "i for a revolute joint qi = di for a prismatic joint
1 Parallel link and serial/parallel hybrid structures are possible, though much less common in industrial manipulators.

3 MANIPULATOR KINEMATICS

16

(a) Standard form

(b) Modied form Figure 8: Different forms of Denavit-Hartenberg notation. and generalized forces Qi = #i fi for a revolute joint for a prismatic joint

representing each link’s coordinate frame with respect to the previous link’s coordinate system; that is 0 Ti = 0 Ti1 i1 Ai (2) where 0 Ti is the homogeneous transformation describing the pose of coordinate frame i with respect to the world coordinate system 0. Two differing methodologies have been established for assigning coordinate frames, each of which allows some freedom in the actual coordinate frame attachment: 1. Frame i has its origin along the axis of joint i + 1, as described by Paul[1] and Lee[2, 7].

The Denavit-Hartenberg (DH) representation results in a 4x4 homogeneous transformation matrix cos"i sin "i cos!i sin "i sin !i ai cos "i sin "i cos "i cos!i cos "i sin !i ai sin "i i1 Ai = (1) 0 sin !i cos !i di 0 0 0 1

3 MANIPULATOR KINEMATICS

17

2. Frame i has its origin along the axis of joint i, and is frequently referred to as ‘modied Denavit-Hartenberg’ (MDH) form[8]. This form is commonly used in literature dealing with manipulator dynamics. The link transform matrix for this form differs from (1). Figure 8 shows the notational differences between the two forms. Note that a i is always the length of link i, but is the displacement between the origins of frame i and frame i + 1 in one convention, and frame i 1 and frame i in the other 2. The Toolbox provides kinematic functions for both of these conventions — those for modied DH parameters are prexed by ‘m’.

3.1 Forward and inverse kinematics
For an n-axis rigid-link manipulator, the forward kinematic solution gives the coordinate frame, or pose, of the last link. It is obtained by repeated application of (2)
0

Tn

= =

0

A1 1 A2 n1 An K (q)

(3) (4)

which is the product of the coordinate frame transform matrices for each link. The pose of the end-effector has 6 degrees of freedom in Cartesian space, 3 in translation and 3 in rotation, so robot manipulators commonly have 6 joints or degrees of freedom to allow arbitrary end-effector pose. The overall manipulator transform 0 Tn is frequently written as Tn , or T6 for a 6-axis robot. The forward kinematic solution may be computed for any manipulator, irrespective of the number of joints or kinematic structure. Of more use in manipulator path planning is the inverse kinematic solution q = K 1 (T) (5)

which gives the joint angles required to reach the specied end-effector position. In general this solution is non-unique, and for some classes of manipulator no closed-form solution exists. If the manipulator has more than 6 joints it is said to be redundant and the solution for joint angles is under-determined. If no solution can be determined for a particular manipulator pose that conguration is said to be singular. The singularity may be due to an alignment of axes reducing the effective degrees of freedom, or the point T being out of reach. The manipulator Jacobian matrix, J " , transforms velocities in joint space to velocities of the end-effector in Cartesian space. For an n-axis manipulator the end-effector Cartesian velocity is
0 tn

xn ˙ xn ˙

= =

0

J" q ˙ J" q ˙

(6) (7)

tn

in base or end-effector coordinates respectively and where x is the Cartesian velocity represented by a 6-vector. For a 6-axis manipulator the Jacobian is square and provided it is not singular can be inverted to solve for joint rates in terms of end-effector Cartesian rates. The Jacobian will not be invertible at a kinematic singularity, and in practice will be poorly
2 Many papers when tabulating the ‘modied’ kinematic parameters of manipulators list a i1 and !i1 not ai and !i .

4 MANIPULATOR RIGID-BODY DYNAMICS

18

conditioned in the vicinity of the singularity, resulting in high joint rates. A control scheme based on Cartesian rate control q = 0 J1 0 xn ˙ ˙ (8) " was proposed by Whitney[9] and is known as resolved rate motion control. For two frames A and B related by A TB = [n o a p] the Cartesian velocity in frame A may be transformed to frame B by B x = B JA A x ˙ ˙ (9) where the Jacobian is given by Paul[10] as
B

JA = f (A TB ) =

[n o a]T 0

[p × n p × o p × a] T [n o a]T

(10)

4 Manipulator rigid-body dynamics
Manipulator dynamics is concerned with the equations of motion, the way in which the manipulator moves in response to torques applied by the actuators, or external forces. The history and mathematics of the dynamics of serial-link manipulators is well covered by Paul[1] and Hollerbach[11]. There are two problems related to manipulator dynamics that are important to solve: inverse dynamics in which the manipulator’s equations of motion are solved for given motion to determine the generalized forces, discussed further in Section 4.1, and direct dynamics in which the equations of motion are integrated to determine the generalized coordinate response to applied generalized forces discussed further in Section 4.2. The equations of motion for an n-axis manipulator are given by q ˙ ˙ ˙ Q = M(q)¨ + C(q, q)q + F(q) + G(q) where q q ˙ q ¨ M C F G Q is the vector of generalized joint coordinates describing the pose of the manipulator is the vector of joint velocities; is the vector of joint accelerations is the symmetric joint-space inertia matrix, or manipulator inertia tensor describes Coriolis and centripetal effects — Centripetal torques are proportional to q 2 , ˙i while the Coriolis torques are proportional to q i q j ˙ ˙ describes viscous and Coulomb friction and is not generally considered part of the rigidbody dynamics is the gravity loading is the vector of generalized forces associated with the generalized coordinates q. (11)

The equations may be derived via a number of techniques, including Lagrangian (energy based), Newton-Euler, d’Alembert[2, 12] or Kane’s[13] method. The earliest reported work was by Uicker[14] and Kahn[15] using the Lagrangian approach. Due to the enormous computational cost, O(n 4 ), of this approach it was not possible to compute manipulator torque for real-time control. To achieve real-time performance many approaches were suggested, including table lookup[16] and approximation[17, 18]. The most common approximation was to ignore the velocity-dependent term C, since accurate positioning and high speed motion are exclusive in typical robot applications.

4 MANIPULATOR RIGID-BODY DYNAMICS

19

Method Lagrangian[22]

Multiplications
5 32 1 n4 + 86 12 n3 2 +171 1 n2 + 53 1 n 4 3 128 150n 48

Additions 25n4 + 66 1 n3 3 +129 1 n2 + 42 1 n 2 3 96 131n 48

For N=6 Multiply Add 66,271 51,548

Recursive NE[22] Kane[13] Simplied RNE[25]

852 646 224

738 394 174

Table 1: Comparison of computational costs for inverse dynamics from various sources. The last entry is achieved by symbolic simplication using the software package ARM. Orin et al.[19] proposed an alternative approach based on the Newton-Euler (NE) equations of rigid-body motion applied to each link. Armstrong[20] then showed how recursion might be applied resulting in O(n) complexity. Luh et al.[21] provided a recursive formulation of the Newton-Euler equations with linear and angular velocities referred to link coordinate frames. They suggested a time improvement from 7.9s for the Lagrangian formulation to 4.5 ms, and thus it became practical to implement ‘on-line’. Hollerbach[22] showed how recursion could be applied to the Lagrangian form, and reduced the computation to within a factor of 3 of the recursive NE. Silver[23] showed the equivalence of the recursive Lagrangian and Newton-Euler forms, and that the difference in efciency is due to the representation of angular velocity. “Kane’s equations” [13] provide another methodology for deriving the equations of motion for a specic manipulator. A number of ‘Z’ variables are introduced, which while not necessarily of physical signicance, lead to a dynamics formulation with low computational burden. Wampler[24] discusses the computational costs of Kane’s method in some detail. The NE and Lagrange forms can be written generally in terms of the Denavit-Hartenberg parameters — however the specic formulations, such as Kane’s, can have lower computational cost for the specic manipulator. Whilst the recursive forms are computationally more efcient, the non-recursive forms compute the individual dynamic terms (M, C and G) directly. A comparison of computation costs is given in Table 1.

4.1 Recursive Newton-Euler formulation
The recursive Newton-Euler (RNE) formulation[21] computes the inverse manipulator dynamics, that is, the joint torques required for a given set of joint angles, velocities and accelerations. The forward recursion propagates kinematic information — such as angular velocities, angular accelerations, linear accelerations — from the base reference frame (inertial frame) to the end-effector. The backward recursion propagates the forces and moments exerted on each link from the end-effector of the manipulator to the base reference frame3. Figure 9 shows the variables involved in the computation for one link. The notation of Hollerbach[22] and Walker and Orin [26] will be used in which the left superscript indicates the reference coordinate frame for the variable. The notation of Luh et al.[21] and later Lee[7, 2] is considerably less clear.
should be noted that using MDH notation with its different axis assignment conventions the Newton Euler formulation is expressed differently[8].
3 It

4 MANIPULATOR RIGID-BODY DYNAMICS

20

Figure 9: Notation used for inverse dynamics, based on standard Denavit-Hartenberg notation. Outward recursion, 1 ≤ i ≤ n. If axis i + 1 is rotational
i+1

$i+1 $i+1 vi+1 vi+1 ˙

= = = =

i+1 i+1 i+1

Ri Ri

i

$i + z0 qi+1 ˙ ˙ ¨ ˙ $i + z0 qi+1 + i $i × z0 qi+1
i+1

(12) (13) (14) + i+1 Ri i vi $i+1 × i+1 p ˙ i+1 (15) (16) (17) (18)
i+1

i+1 ˙ i+1 i+1

i

$i+1 × i+1 p + i+1 Ri i vi i+1 $i+1 × i+1 p + i+1 $i+1 × i+1

i+1 ˙

i+1

$i+1 $i+1 vi+1 vi+1 ˙

If axis i + 1 is translational = i+1 Ri i $i = = =
i+1 i+1 i+1

i+1 ˙ i+1 i+1

˙ R i $i Ri z0 qi+1 + i vi + i+1 $i+1 × i+1 p ˙ i+1 ˙ Ri z0 qi+1 + i vi + i+1 $i+1 × i+1 p + 2 i+1 $i+1 × ¨ ˙ i+1
i+1

i

Ri z0 qi+1 ˙ (19) (20) (21) (22)

+i+1 $i+1 ×
i˙ i i

$i+1 × i+1 p i+1
i

vi

Fi Ni

˙ ˙ $i × si + i $i × i $i × si + i vi i˙ = mi vi ˙ = J i i $ i + i $i × J i i $i =

Inward recursion, n ≥ i ≥ 1.
i i

fi ni

= = =

i i

Ri+1 i+1 f i+1 + i F i

(23) (24) (25)

Qi where

Ri+1 i+1 ni+1 + i+1 Ri i p × ii+1 f i+1 + i p + si × i F i + i N i i i T i i ni if link i + 1 is rotational Ri+1 z0
if T i iR i+1 z0

if link i + 1 is translational

4 MANIPULATOR RIGID-BODY DYNAMICS

21

i Ji si $i ˙ $i vi vi ˙ vi ˙ vi ni fi Ni Fi Qi i1 R i

is the link index, in the range 1 to n is the moment of inertia of link i about its COM is the position vector of the COM of link i with respect to frame i is the angular velocity of link i is the angular acceleration of link i is the linear velocity of frame i is the linear acceleration of frame i is the linear velocity of the COM of link i is the linear acceleration of the COM of link i is the moment exerted on link i by link i 1 is the force exerted on link i by link i 1 is the total moment at the COM of link i is the total force at the COM of link i is the force or torque exerted by the actuator at joint i is the orthonormal rotation matrix dening frame i orientation with respect to frame i 1. It is the upper 3 × 3 portion of the link transform matrix given in (1). sin !i sin "i cos "i cos !i sin "i i1 (26) Ri = sin "i cos !i cos "i sin !i cos "i 0 sin !i cos !i
i

Ri1

= (i1 Ri )1 = (i1 Ri )T

(27)

i p i

is the displacement from the origin of frame i 1 to frame i with respect to frame i. ai i (28) pi = di sin !i di cos !i It is the negative translational part of ( i1 Ai )1 . is a unit vector in Z direction, z0 = [0 0 1]

z0

Note that the COM linear velocity given by equation (14) or (18) does not need to be computed since no other expression depends upon it. Boundary conditions are used to introduce the effect of gravity by setting the acceleration of the base link v0 = g ˙ (29)

where g is the gravity vector in the reference coordinate frame, generally acting in the negative Z direction, downward. Base velocity is generally zero v0 $0 ˙ $0 = = = 0 0 0 (30) (31) (32)

At this stage the Toolbox only provides an implementation of this algorithm using the standard Denavit-Hartenberg conventions.

4.2 Direct dynamics
Equation (11) may be used to compute the so-called inverse dynamics, that is, actuator torque as a function of manipulator state and is useful for on-line control. For simulation

REFERENCES

22

the direct, integral or forward dynamic formulation is required giving joint motion in terms of input torques. Walker and Orin[26] describe several methods for computing the forward dynamics, and all make use of an existing inverse dynamics solution. Using the RNE algorithm for inverse dynamics, the computational complexity of the forward dynamics using ‘Method 1’ is O(n3 ) for an n-axis manipulator. Their other methods are increasingly more sophisticated but reduce the computational cost, though still O(n 3 ). Featherstone[27] has described the “articulated-body method” for O(n) computation of forward dynamics, however for n < 9 it is more expensive than the approach of Walker and Orin. Another O(n) approach for forward dynamics has been described by Lathrop[28].

4.3 Rigid-body inertial parameters
Accurate model-based dynamic control of a manipulator requires knowledge of the rigidbody inertial parameters. Each link has ten independent inertial parameters: link mass, mi ; three rst moments, which may be expressed as the COM location, s i , with respect to some datum on the link or as a moment Si = mi si ; six second moments, which represent the inertia of the link about a given axis, typically through the COM. The second moments may be expressed in matrix or tensor form as Jxx Jxy Jxz J = Jxy Jyy Jyz (33) Jxz Jyz Jzz where the diagonal elements are the moments of inertia, and the off-diagonals are products of inertia. Only six of these nine elements are unique: three moments and three products of inertia. For any point in a rigid-body there is one set of axes known as the principal axes of inertia for which the off-diagonal terms, or products, are zero. These axes are given by the eigenvectors of the inertia matrix (33) and the eigenvalues are the principal moments of inertia. Frequently the products of inertia of the robot links are zero due to symmetry. A 6-axis manipulator rigid-body dynamic model thus entails 60 inertial parameters. There may be additional parameters per joint due to friction and motor armature inertia. Clearly, establishing numeric values for this number of parameters is a difcult task. Many parameters cannot be measured without dismantling the robot and performing careful experiments, though this approach was used by Armstrong et al.[29]. Most parameters could be derived from CAD models of the robots, but this information is often considered proprietary and not made available to researchers.

References
[1] R. P. Paul, Robot Manipulators: Mathematics, Programming, and Control. Cambridge, Massachusetts: MIT Press, 1981.

REFERENCES

23

[2] K. S. Fu, R. C. Gonzalez, and C. S. G. Lee, Robotics. Control, Sensing, Vision and Intelligence. McGraw-Hill, 1987. [3] M. Spong and M. Vidyasagar, Robot Dynamics and Control. John Wiley and Sons, 1989. [4] J. J. Craig, Introduction to Robotics. Addison Wesley, 1986. [5] S. Hutchinson, G. Hager, and P. Corke, “A tutorial on visual servo control,” IEEE Transactions on Robotics and Automation, vol. 12, pp. 651–670, Oct. 1996. [6] R. S. Hartenberg and J. Denavit, “A kinematic notation for lower pair mechanisms based on matrices,” Journal of Applied Mechanics, vol. 77, pp. 215–221, June 1955. [7] C. S. G. Lee, “Robot arm kinematics, dynamics and control,” IEEE Computer, vol. 15, pp. 62–80, Dec. 1982. [8] J. J. Craig, Introduction to Robotics. Addison Wesley, second ed., 1989. [9] D. Whitney, “The mathematics of coordinated control of prosthetic arms and manipulators,” ASME Journal of Dynamic Systems, Measurement and Control, vol. 20, no. 4, pp. 303–309, 1972. [10] R. P. Paul, B. Shimano, and G. E. Mayer, “Kinematic control equations for simple manipulators,” IEEE Trans. Syst. Man Cybern., vol. 11, pp. 449–455, June 1981. [11] J. M. Hollerbach, “Dynamics,” in Robot Motion - Planning and Control (M. Brady, J. M. Hollerbach, T. L. Johnson, T. Lozano-Perez, and M. T. Mason, eds.), pp. 51–71, MIT, 1982. [12] C. S. G. Lee, B. Lee, and R. Nigham, “Development of the generalized D’Alembert equations of motion for mechanical manipulators,” in Proc. 22nd CDC, (San Antonio, Texas), pp. 1205–1210, 1983. [13] T. Kane and D. Levinson, “The use of Kane’s dynamical equations in robotics,” Int. J. Robot. Res., vol. 2, pp. 3–21, Fall 1983. [14] J. Uicker, On the Dynamic Analysis of Spatial Linkages Using 4 by 4 Matrices. PhD thesis, Dept. Mechanical Engineering and Astronautical Sciences, NorthWestern University, 1965. [15] M. Kahn, “The near-minimum time control of open-loop articulated kinematic linkages,” Tech. Rep. AIM-106, Stanford University, 1969. [16] M. H. Raibert and B. K. P. Horn, “Manipulator control using the conguration space method,” The Industrial Robot, pp. 69–73, June 1978. [17] A. Bejczy, “Robot arm dynamics and control,” Tech. Rep. NASA-CR-136935, NASA JPL, Feb. 1974. [18] R. Paul, “Modelling, trajectory calculation and servoing of a computer controlled arm,” Tech. Rep. AIM-177, Stanford University, Articial Intelligence Laboratory, 1972. [19] D. Orin, R. McGhee, M. Vukobratovic, and G. Hartoch, “Kinematics and kinetic analysis of open-chain linkages utilizing Newton-Euler methods,” Mathematical Biosciences. An International Journal, vol. 43, pp. 107–130, Feb. 1979.

REFERENCES

24

[20] W. Armstrong, “Recursive solution to the equations of motion of an n-link manipulator,” in Proc. 5th World Congress on Theory of Machines and Mechanisms, (Montreal), pp. 1343–1346, July 1979. [21] J. Y. S. Luh, M. W. Walker, and R. P. C. Paul, “On-line computational scheme for mechanical manipulators,” ASME Journal of Dynamic Systems, Measurement and Control, vol. 102, pp. 69–76, 1980. [22] J. Hollerbach, “A recursive Lagrangian formulation of manipulator dynamics and a comparative study of dynamics formulation complexity,” IEEE Trans. Syst. Man Cybern., vol. SMC-10, pp. 730–736, Nov. 1980. [23] W. M. Silver, “On the equivalance of Lagrangian and Newton-Euler dynamics for manipulators,” Int. J. Robot. Res., vol. 1, pp. 60–70, Summer 1982. [24] C. Wampler, Computer Methods in Manipulator Kinematics, Dynamics, and Control: a Comparative Study. PhD thesis, Stanford University, 1985. [25] J. J. Murray, Computational Robot Dynamics. PhD thesis, Carnegie-Mellon University, 1984. [26] M. W. Walker and D. E. Orin, “Efcient dynamic computer simulation of robotic mechanisms,” ASME Journal of Dynamic Systems, Measurement and Control, vol. 104, pp. 205–211, 1982. [27] R. Featherstone, Robot Dynamics Algorithms. Kluwer Academic Publishers, 1987. [28] R. Lathrop, “Constrained (closed-loop) robot simulation by local constraint propogation.,” in Proc. IEEE Int. Conf. Robotics and Automation, pp. 689–694, 1986. [29] B. Armstrong, O. Khatib, and J. Burdick, “The explicit dynamic model and inertial parameters of the Puma 560 arm,” in Proc. IEEE Int. Conf. Robotics and Automation, vol. 1, (Washington, USA), pp. 510–18, 1986.

1

2
Reference
For an n-axis manipulator the following matrix naming and dimensional conventions apply. Symbol
l q q qd qd qdd qdd robot T T Q M

Dimensions link 1×n m×n 1×n m×n 1×n m×n robot 4×4 4×4×m quaternion 1×6

v t d

3×1 m×1 6×1

Description manipulator link object joint coordinate vector m-point joint coordinate trajectory joint velocity vector m-point joint velocity trajectory joint acceleration vector m-point joint acceleration trajectory robot object homogeneous transform m-point homogeneous transform trajectory unit-quaternion object vector with elements of 0 or 1 corresponding to Cartesian DOF along X, Y, Z and around X, Y, Z. 1 if that Cartesian DOF belongs to the task space, else 0. Cartesian vector time vector differential motion vector

Object names are shown in bold typeface. A trajectory is represented by a matrix in which each row corresponds to one of m time steps. For a joint coordinate, velocity or acceleration trajectory the columns correspond to the robot axes. For homogeneous transform trajectories we use 3-dimensional matrices where the last index corresponds to the time step.

Units
All angles are in radians. The choice of all other units is up to the user, and this choice will ow on to the units in which homogeneous transforms, Jacobians, inertias and torques are represented.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

Introduction

2

Homogeneous Transforms
eul2tr oa2tr rotvec rotx roty rotz rpy2tr tr2eul tr2rot tr2rpy transl trnorm Euler angle to homogeneous transform orientation and approach vector to homogeneous transform homogeneous transform for rotation about arbitrary vector homogeneous transform for rotation about X-axis homogeneous transform for rotation about Y-axis homogeneous transform for rotation about Z-axis Roll/pitch/yaw angles to homogeneous transform homogeneous transform to Euler angles homogeneous transform to rotation submatrix homogeneous transform to roll/pitch/yaw angles set or extract the translational component of a homogeneous transform normalize a homogeneous transform

Trajectory Generation
ctraj jtraj trinterp Cartesian trajectory joint space trajectory interpolate homogeneous transforms

Quaternions
/ * inv norm plot q2tr quaternion qinterp unit divide quaternion by quaternion or scalar multiply quaternion by a quaternion or vector invert a quaternion norm of a quaternion display a quaternion as a 3D rotation quaternion to homogeneous transform construct a quaternion interpolate quaternions unitize a quaternion

Manipulator Models
link nofriction perturb puma560 puma560akb robot showlink stanford twolink construct a robot link object remove friction from a robot object randomly modify some dynamic parameters Puma 560 data Puma 560 data (modied Denavit-Hartenberg) construct a robot object show link/robot data in detail Stanford arm data simple 2-link example

Robotics Toolbox Release 7.1

Peter Corke, April 2002

Introduction

3

Kinematics
diff2tr fkine ftrans ikine ikine560 jacob0 jacobn tr2diff tr2jac differential motion vector to transform compute forward kinematics transform force/moment compute inverse kinematics compute inverse kinematics for Puma 560 like arm compute Jacobian in base coordinate frame compute Jacobian in end-effector coordinate frame homogeneous transform to differential motion vector homogeneous transform to Jacobian

Graphics
drivebot plot drive a graphical robot plot/animate robot

Dynamics
accel cinertia coriolis fdyn friction gravload inertia itorque rne compute forward dynamics compute Cartesian manipulator inertia matrix compute centripetal/coriolis torque forward dynamics (motion given forces) joint friction compute gravity loading compute manipulator inertia matrix compute inertia torque inverse dynamics (forces given motion)

Other
ishomog maniplty rtdemo unit test if matrix is 4 × 4 compute manipulability toolbox demonstration unitize a vector

Robotics Toolbox Release 7.1

Peter Corke, April 2002

accel

4

accel
Purpose Synopsis Description
Compute manipulator forward dynamics

qdd = accel(robot, q, qd, torque)

Returns a vector of joint accelerations that result from applying the actuator torque to the manipulator robot with joint coordinates q and velocities qd.

Algorithm

Uses the method 1 of Walker and Orin to compute the forward dynamics. This form is useful for simulation of manipulator dynamics, in conjunction with a numerical integration function.

See Also References

rne, robot, fdyn, ode45

M. W. Walker and D. E. Orin. Efcient dynamic computer simulation of robotic mechanisms. ASME Journal of Dynamic Systems, Measurement and Control, 104:205–211, 1982.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

cinertia

5

cinertia
Purpose Synopsis Description
Compute the Cartesian (operational space) manipulator inertia matrix

M = cinertia(robot, q)

cinertia computes the Cartesian, or operational space, inertia matrix. robot is a robot object that describes the manipulator dynamics and kinematics, and q is an n-element vector of joint coordinates.

Algorithm

The Cartesian inertia matrix is calculated from the joint-space inertia matrix by M(x) = J(q)T M(q)J(q)1 and relates Cartesian force/torque to Cartesian acceleration F = M(x)x ¨

See Also References

inertia, robot, rne

O. Khatib, “A unied approach for motion and force control of robot manipulators: the operational space formulation,” IEEE Trans. Robot. Autom., vol. 3, pp. 43–53, Feb. 1987.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

coriolis

6

coriolis
Purpose Synopsis Description
Compute the manipulator Coriolis/centripetal torque components

tau c = coriolis(robot, q, qd)

coriolis returns the joint torques due to rigid-body Coriolis and centripetal effects for the specied joint state q and velocity qd. robot is a robot object that describes the manipulator dynamics and kinematics.

If q and qd are row vectors, tau c is a row vector of joint torques. If q and qd are matrices, each row is interpreted as a joint state vector, and tau c is a matrix each row being the corresponding joint torques.

Algorithm

Evaluated from the equations of motion, using rne, with joint acceleration and gravitational acceleration set to zero, ˙ ˙ # = C(q, q)q Joint friction is ignored in this calculation.

See Also References

robot, rne, itorque, gravload

M. W. Walker and D. E. Orin. Efcient dynamic computer simulation of robotic mechanisms. ASME Journal of Dynamic Systems, Measurement and Control, 104:205–211, 1982.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

ctraj

7

ctraj
Purpose Synopsis
Compute a Cartesian trajectory between two points

TC = ctraj(T0, T1, m) TC = ctraj(T0, T1, r)

Description

ctraj returns a Cartesian trajectory (straight line motion) TC from the point represented by homogeneous transform T0 to T1. The number of points along the path is m or the length of the given vector r. For the second case r is a vector of distances along the path (in the range 0 to 1) for each point. The rst case has the points equally spaced, but different spacing may be specied to achieve acceptable acceleration prole. TC is a 4 × 4 × m matrix.

Examples

To create a Cartesian path with smooth acceleration we can use the jtraj function to create the path vector r with continuous derivitives. >> >> >> >> >> T0 = transl([0 0 0]); T1 = transl([-1 2 1]); t= [0:0.056:10]; r = jtraj(0, 1, t); TC = ctraj(T0, T1, r); plot(t, transl(TC));
2

1.5

1

0.5

0

!0.5

!1

0

1

2

3

4

5 Time (s)

6

7

8

9

10

See Also References

trinterp, qinterp, transl

R. P. Paul, Robot Manipulators: Mathematics, Programming, and Control. Cambridge, Massachusetts: MIT Press, 1981.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

diff2tr

8

diff2tr
Purpose Synopsis Description
Convert a differential motion vector to a homogeneous transform

delta = diff2tr(x)

Returns a homogeneous transform representing differential translation and rotation corresponding to Cartesian velocity x = [v x vy vz $x $y $z ]. From mechanics we know that

Algorithm

˙ R = Sk(%)R $y $x 0

where R is an orthonormal rotation matrix and 0 $z 0 Sk(%) = $z $y $x ˙ T= for the rotational and translational case. Sk(%) 000 ˙ P 1

and is a skew-symmetric matrix. This can be generalized to T

See Also References

tr2diff

R. P. Paul. Robot Manipulators: Mathematics, Programming, and Control. MIT Press, Cambridge, Massachusetts, 1981.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

drivebot

9

drivebot
Purpose Synopsis
Drive a graphical robot

drivebot(robot) drivebot(robot, q)

Description

Pops up a window with one slider for each joint. Operation of the sliders will drive the graphical robot on the screen. Very useful for gaining an understanding of joint limits and robot workspace. The joint coordinate state is kept with the graphical robot and can be obtained using the
plot function. If q is specied it is used as the initial joint angle, otherwise the initial value

of joint coordinates is taken from the graphical robot.

Examples

To drive a graphical Puma 560 robot >> puma560 >> plot(p560,qz) >> drivebot(p560) % define the robot % draw it % now drive it

See Also

robot/plot,robot

Robotics Toolbox Release 7.1

Peter Corke, April 2002

eul2tr

10

eul2tr
Purpose Synopsis
Convert Euler angles to a homogeneous transform

T = eul2tr([r p y]) T = eul2tr(r,p,y)

Description

eul2tr returns a homogeneous transformation for the specied Euler angles in radians.

T = RZ (a)RY (b)RZ (c)

Cautionary

Note that 12 different Euler angle sets or conventions exist. The convention used here is the common one for robotics, but is not the one used for example in the aerospace community.

See Also References

tr2eul, rpy2tr

R. P. Paul, Robot Manipulators: Mathematics, Programming, and Control. Cambridge, Massachusetts: MIT Press, 1981.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

fdyn

11

fdyn
Purpose Synopsis
Integrate forward dynamics

[t q qd] = fdyn(robot, t0, t1) [t q qd] = fdyn(robot, t0, t1, torqfun) [t q qd] = fdyn(robot, t0, t1, torqfun, q0, qd0) [t q qd] = fdyn(robot, t0, t1, torqfun, q0, qd0, arg1, arg2, ...)

Description

fdyn integrates the manipulator equations of motion over the time interval t0 to t1 using MATLAB’s ode45 numerical integration function. Manipulator kinematic and dynamic chacteristics are given by the robot object robot. It returns a time vector t, and matrices of manipulator joint state q and joint velocities qd. These matrices have one row per time step and one column per joint.

Actuator torque may be specied by a user function
tau = torqfun(t, q, qd, arg1, arg2, ...)

where t is the current time, and q and qd are the manipulator joint coordinate and velocity state respectively. Optional arguments passed to fdyn will be passed through to the user function. Typically this function would be used to implement some axis control scheme as a function of manipulator state and passed in setpoint information. If torqfun is not specied then zero torque is applied to the manipulator. Initial joint coordinates and velocities may be specied by the optional arguments q0 and
qd0 respectively.

Algorithm

The joint acceleration is a function of joint coordinate and velocity given by q = M(q)1 # C(q, q)q G(q) F(q) ¨ ˙ ˙ ˙

Example

The following example shows how fdyn() can be used to simulate a robot and its controller. The manipulator is a Puma 560 with simple proportional and derivative controller. The simulation results are shown in the gure, and further gain tuning is clearly required. Note that high gains are required on joints 2 and 3 in order to counter the signicant disturbance torque due to gravity.
>> >> >> >> >> >> >> puma560 % load Puma parameters t = [0:.056:5]’; % time vector q_dmd = jtraj(qz, qr,t); % create a path qt = [t q_dmd]; Pgain = [20 100 20 5 5 5]; % set gains Dgain = [-5 -10 -2 0 0 0]; [tsim,q,qd] = fdyn(nofriction(p560), 0, 5, ’taufunc’, qz, qz, Pgain, Dgain, qt);

Robotics Toolbox Release 7.1

Peter Corke, April 2002

fdyn

12

Note the use of qz a zero vector of length 6 dened by puma560 pads out the two initial condition arguments, and we place the control gains and the path as optional arguments. Note also the use of the nofriction() function, see Cautionary note below. The invoked function is % % taufunc.m % % user written function to compute joint torque as a function % of joint error. The desired path is passed in via the global % matrix qt. The controller implemented is PD with the proportional % and derivative gains given by the global variables Pgain and Dgain % respectively. % function tau = taufunc(t, q, qd, Pgain, Dgain, qt) % interpolate demanded angles for this time if t > qt(end,1), % keep time in range t = qt(end,1); end q_dmd = interp1(qt(:,1), qt(:,2:7), t)’; % compute error and joint torque e = q_dmd - q; tau = diag(Pgain)*e + diag(Dgain)*qd;
0.05
Joint 1 (rad)

0

!0.05 0 2
Joint 2 (rad)

0.5

1

1.5

2

2.5 Time (s)

3

3.5

4

4.5

5

1 0 !1 0 1 0.5 1 1.5 2 2.5 Time (s) 3 3.5 4 4.5 5

Joint 3 (rad)

0 !1 !2 0 0.5 1 1.5 2 2.5 Time (s) 3 3.5 4 4.5 5

Results of fdyn() example. Simulated path shown as solid, and reference path as dashed.

Cautionary

The presence of non-linear friction in the dynamic model can prevent the integration from converging. The function nofriction() can be used to return a Coulomb friction free robot object.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

fdyn

13

See Also References

accel, nofriction, rne, robot, ode45

M. W. Walker and D. E. Orin. Efcient dynamic computer simulation of robotic mechanisms. ASME Journal of Dynamic Systems, Measurement and Control, 104:205–211, 1982.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

fkine

14

fkine
Purpose Synopsis Description
Forward robot kinematics for serial link manipulator

T = fkine(robot, q)

fkine computes forward kinematics for the joint coordinate q giving a homogeneous transform for the location of the end-effector. robot is a robot object which contains a kinematic model in either standard or modied Denavit-Hartenberg notation. Note that the robot object can specify an arbitrary homogeneous transform for the base of the robot and a tool offset. If q is a vector it is interpreted as the generalized joint coordinates, and fkine returns a homogeneous transformation for the nal link of the manipulator. If q is a matrix each row is interpreted as a joint state vector, and T is a 4 × 4 × m matrix where m is the number of rows in q.

Cautionary

Note that the dimensional units for the last column of the T matrix will be the same as the dimensional units used in the robot object. The units can be whatever you choose (metres, inches, cubits or furlongs) but the choice will affect the numerical value of the elements in the last column of T. The Toolbox denitions puma560 and stanford all use SI units with dimensions in metres.

See Also References

link, robot

R. P. Paul. Robot Manipulators: Mathematics, Programming, and Control. MIT Press, Cambridge, Massachusetts, 1981. J. J. Craig, Introduction to Robotics. Addison Wesley, second ed., 1989.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

link/friction

15

link/friction
Purpose Synopsis Description
Compute joint friction torque

tau f = friction(link, qd)

friction computes the joint friction torque based on friction parameter data, if any, in the link object link. Friction is a function only of joint velocity qd If qd is a vector then tau f is a vector in which each element is the friction torque for the the corresponding element in qd.

Algorithm

The friction model is a fairly standard one comprising viscous friction and direction dependent Coulomb friction ˙ Bi q + # , " < 0 ˙ i Fi (t) = + ˙ ˙ Bi q + #i , " > 0

See Also

link,robot/friction,nofriction

Robotics Toolbox Release 7.1

Peter Corke, April 2002

robot/friction

16

robot/friction
Purpose Synopsis Description
Compute robot friction torque vector

tau f = friction(robot, qd)

friction computes the joint friction torque vector for the robot object robot with a joint velocity vector qd.

See Also

link, link/friction, nofriction

Robotics Toolbox Release 7.1

Peter Corke, April 2002

ftrans

17

ftrans
Purpose Synopsis Description
Force transformation

F2 = ftrans(F, T)

Transform the force vector F in the current coordinate frame to force vector F2 in the second coordinate frame. The second frame is related to the rst by the homogeneous transform T. F2 and F are each 6-element vectors comprising force and moment components [Fx Fy Fz Mx My Mz ].

See Also

diff2tr

Robotics Toolbox Release 7.1

Peter Corke, April 2002

gravload

18

gravload
Purpose Synopsis
Compute the manipulator gravity torque components

tau g = gravload(robot, q) tau g = gravload(robot, q, grav)

Description

gravload computes the joint torque due to gravity for the manipulator in pose q. If q is a row vector, tau g returns a row vector of joint torques. If q is a matrix each row is interpreted as as a joint state vector, and tau g is a matrix in which each row is the gravity torque for the the corresponding row in q. The default gravity direction comes from the robot object but may be overridden by the optional grav argument.

See Also References

robot, link, rne, itorque, coriolis

M. W. Walker and D. E. Orin. Efcient dynamic computer simulation of robotic mechanisms. ASME Journal of Dynamic Systems, Measurement and Control, 104:205–211, 1982.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

ikine

19

ikine
Purpose Synopsis
Inverse manipulator kinematics

q = ikine(robot, T) q = ikine(robot, T, q0) q = ikine(robot, T, q0, M)

Description

ikine returns the joint coordinates for the manipulator described by the object robot whose endeffector homogeneous transform is given by T. Note that the robot’s base can be arbitrarily specied within the robot object. If T is a homogeneous transform then a row vector of joint coordinates is returned. The estimate for the rst step is q0 if this is given else 0. If T is a homogeneous transform trajectory of size 4 × 4 × m then q will be an n × m matrix where each row is a vector of joint coordinates corresponding to the last subscript of T. The estimate for the rst step in the sequence is q0 if this is given else 0. The initial estimate of q for each time step is taken as the solution from the previous time step. Note that the inverse kinematic solution is generally not unique, and depends on the initial value q0 (which defaults to 0). For the case of a manipulator with fewer than 6 DOF it is not possible for the end-effector to satisfy the end-effector pose specied by an arbitrary homogeneous transform. This typically leads to nonconvergence in ikine. A solution is to specify a 6-element weighting vector, M, whose elements are 0 for those Cartesian DOF that are unconstrained and 1 otherwise. The elements correspond to translation along the X-, Y- and Z-axes and rotation about the X-, Y- and Z-axes respectively. For example, a 5-axis manipulator may be incapable of independantly controlling rotation about the end-effector’s Z-axis. In this case M = [1 1 1 1 1 0] would enable a solution in which the endeffector adopted the pose T except for the end-effector rotation. The number of non-zero elements should equal the number of robot DOF.

Algorithm

The solution is computed iteratively using the pseudo-inverse of the manipulator Jacobian. q = J+ (q)& F (q) T ˙ where & returns the ‘difference’ between two transforms as a 6-element vector of displacements and rotations (see tr2diff).

Cautionary

Such a solution is completely general, though much less efcient than specic inverse kinematic solutions derived symbolically. The returned joint angles may be in non-minimum form, ie. q + 2n'. This approach allows a solution to obtained at a singularity, but the joint coordinates within the null space are arbitrarily assigned.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

ikine

20

Note that the dimensional units used for the last column of the T matrix must agree with the dimensional units used in the robot denition. These units can be whatever you choose (metres, inches, cubits or furlongs) but they must be consistent. The Toolbox denitions puma560 and stanford all use SI units with dimensions in metres.

See Also References

fkine, tr2diff, jacob0, ikine560, robot

S. Chieaverini, L. Sciavicco, and B. Siciliano, “Control of robotic systems through singularities,” in Proc. Int. Workshop on Nonlinear and Adaptive Control: Issues in Robotics (C. C. de Wit, ed.), Springer-Verlag, 1991.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

ikine560

21

ikine560
Purpose Synopsis Description
Inverse manipulator kinematics for Puma 560 like arm

q = ikine560(robot, config)

ikine560 returns the joint coordinates corresponding to the end-effector homogeneous transform T. It is computed using a symbolic solution appropriate for Puma 560 like robots, that is, all revolute 6DOF arms, with a spherical wrist. The use of a symbolic solution means that it executes over 50 times faster than ikine for a Puma 560 solution. A further advantage is that ikine560() allows control over the specic solution returned. config is a string which contains one or more of the conguration control letter codes ’l’ ’r’ ’u’ ’d’ ’f’ ’n’ left-handed (lefty) solution (default) right-handed (righty) solution elbow up solution (default) elbow down solution wrist ipped solution wrist not ipped solution (default)

Cautionary

Note that the dimensional units used for the last column of the T matrix must agree with the dimensional units used in the robot object. These units can be whatever you choose (metres, inches, cubits or furlongs) but they must be consistent. The Toolbox denitions puma560 and stanford all use SI units with dimensions in metres.

See Also References

fkine, ikine, robot

R. P. Paul and H. Zhang, “Computationally efcient kinematics for manipulators with spherical wrists,” Int. J. Robot. Res., vol. 5, no. 2, pp. 32–44, 1986.

Author

Robert Biro and Gary McMurray, Georgia Institute of Technology, gt2231a@acmex.gatech.edu

Robotics Toolbox Release 7.1

Peter Corke, April 2002

inertia

22

inertia
Purpose Synopsis Description
Compute the manipulator joint-space inertia matrix

M = inertia(robot, q)

inertia computes the joint-space inertia matrix which relates joint torque to joint acceleration ¨ # = M(q)q robot is a robot object that describes the manipulator dynamics and kinematics, and q is an nelement vector of joint state. For an n-axis manipulator M is an n × n symmetric matrix. If q is a matrix each row is interpreted as a joint state vector, and I is an n × n × m matrix where m is the number of rows in q. Note that if the robot contains motor inertia parameters then motor inertia, referred to the link reference frame, will be added to the diagonal of M.

Example

To show how the inertia ‘seen’ by the waist joint varies as a function of joint angles 2 and 3 the following code could be used. >> >> >> >> [q2,q3] = meshgrid(-pi:0.2:pi, -pi:0.2:pi); q = [zeros(length(q2(:)),1) q2(:) q3(:) zeros(length(q2(:)),3)]; I = inertia(p560, q); surfl(q2, q3, squeeze(I(1,1,:)));

5.5 5 4.5 4

I11

3.5 3 2.5 2 4 2 0 !2 q3 !4 !4 !2 q2 2 0 4

See Also

robot, rne, itorque, coriolis, gravload

Robotics Toolbox Release 7.1

Peter Corke, April 2002

inertia

23

References

M. W. Walker and D. E. Orin. Efcient dynamic computer simulation of robotic mechanisms. ASME Journal of Dynamic Systems, Measurement and Control, 104:205–211, 1982.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

ishomog

24

ishomog
Purpose Synopsis Description
Test if argument is a homogeneous transformation

ishomog(x)

Returns true if x is a 4 × 4 matrix.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

itorque

25

itorque
Purpose Synopsis Description
Compute the manipulator inertia torque component

tau i = itorque(robot, q, qdd)

itorque returns the joint torque due to inertia at the specied pose q and acceleration qdd which is given by ¨ #i = M(q)q If q and qdd are row vectors, itorque is a row vector of joint torques. If q and qdd are matrices, each row is interpreted as a joint state vector, and itorque is a matrix in which each row is the inertia torque for the corresponding rows of q and qdd. robot is a robot object that describes the kinematics and dynamics of the manipulator and drive. If robot contains motor inertia parameters then motor inertia, referred to the link reference frame, will be included in the diagonal of M and inuence the inertia torque result.

See Also

robot, rne, coriolis, inertia, gravload

Robotics Toolbox Release 7.1

Peter Corke, April 2002

jacob0

26

jacob0
Purpose Synopsis Description
Compute manipulator Jacobian in base coordinates

jacob0(robot, q)

jacob0 returns a Jacobian matrix for the robot object robot in the pose q and as expressed in the base coordinate frame. ˙ The manipulator Jacobian matrix, 0 Jq , maps differential velocities in joint space, q, to Cartesian velocity of the end-effector expressed in the base coordinate frame.
0

x = 0 Jq (q)q ˙ ˙

For an n-axis manipulator the Jacobian is a 6 × n matrix.

See Also References

jacobn, diff2tr, tr2diff, robot

R. P. Paul, B. Shimano and G. E. Mayer. Kinematic Control Equations for Simple Manipulators. IEEE Systems, Man and Cybernetics 11(6), pp 449-455, June 1981.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

jacobn

27

jacobn
Purpose Synopsis Description
Compute manipulator Jacobian in end-effector coordinates

jacobn(robot, q)

jacobn returns a Jacobian matrix for the robot object robot in the pose q and as expressed in the end-effector coordinate frame. ˙ The manipulator Jacobian matrix, 0 Jq , maps differential velocities in joint space, q, to Cartesian velocity of the end-effector expressed in the end-effector coordinate frame.
n

x = n Jq (q)q ˙ ˙

The relationship between tool-tip forces and joint torques is given by # = n Jq (q) n F For an n-axis manipulator the Jacobian is a 6 × n matrix.

See Also References

jacob0, diff2tr, tr2diff, robot

R. P. Paul, B. Shimano and G. E. Mayer. Kinematic Control Equations for Simple Manipulators. IEEE Systems, Man and Cybernetics 11(6), pp 449-455, June 1981.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

jtraj

28

jtraj
Purpose Synopsis
Compute a joint space trajectory between two joint coordinate poses

[q [q [q [q

qd qd qd qd

qdd] qdd] qdd] qdd]

= = = =

jtraj(q0, jtraj(q0, jtraj(q0, jtraj(q0,

q1, q1, q1, q1,

n) n, qd0, qd1) t) t, qd0, qd1)

Description

jtraj returns a joint space trajectory q from joint coordinates q0 to q1. The number of points is n or the length of the given time vector t. A 7th order polynomial is used with default zero boundary conditions for velocity and acceleration. Non-zero boundary velocities can be optionally specied as qd0 and qd1. The trajectory is a matrix, with one row per time step, and one column per joint. The function can optionally return a velocity and acceleration trajectories as qd and qdd respectively.

See Also

ctraj

Robotics Toolbox Release 7.1

Peter Corke, April 2002

link

29

link
Purpose Synopsis
Link object

L = link L = link([alpha, a, theta, d], convention) L = link([alpha, a, theta, d, sigma], convention) L = link(dyn row, convention) A = link(q) show(L)

Description

The link function constructs a link object. The object contains kinematic and dynamic parameters as well as actuator and transmission parameters. The rst form returns a default object, while the second and third forms initialize the kinematic model based on Denavit and Hartenberg parameters. The dynamic model can be initialized using the fourth form of the constructor where dyn row is a 1 × 20 matrix which is one row of the legacy dyn matrix. By default the standard Denavit and Hartenberg conventions are assumed but this can be overridden by the optional convention argument which can be set to either ’modified’ or ’standard’ (default). Note that any abbreviation of the string can be used, ie. ’mod’ or even ’m’. The second last form given above is not a constructor but a link method that returns the link transformation matrix for the given joint coordinate. The argument is given to the link object using parenthesis. The single argument is taken as the link variable q and substituted for " or D for a revolute or prismatic link respectively. The Denavit and Hartenberg parameters describe the spatial relationship between this link and the previous one. The meaning of the elds for each kinematic convention are summarized in the following table. variable alpha A theta D sigma DH !i Ai "i Di (i MDH !i1 Ai1 "i Di (i description link twist angle link length link rotation angle link offset distance joint type; 0 for revolute, non-zero for prismatic

Since Matlab does not support the concept of public class variables methods have been written to allow link object parameters to be referenced (r) or assigned (a) as given by the following table

Robotics Toolbox Release 7.1

Peter Corke, April 2002

link

30

Method link.alpha link.A link.theta link.D link.sigma link.RP link.mdh link.I link.I link.m link.r link.G link.Jm link.B link.Tc link.Tc

Operations r+a r+a r+a r+a r+a r r+a r a r+a r+a r+a r+a r+a r a

Returns link twist angle link length link rotation angle link offset distance joint type; 0 for revolute, non-zero for prismatic joint type; ’R’ or ’P’ DH convention: 0 if standard, 1 if modied 3 × 3 symmetric inertia matrix assigned from a 3×3 matrix or a 6-element vector interpretted as [Ixx Iyy Izz Ixy Iyz Ixz ] link mass 3 × 1 link COG vector gear ratio motor inertia viscous friction Coulomb friction, 1 × 2 vector where [#+ # ] Coulomb friction; for symmetric friction this is a scalar, for asymmetric friction it is a 2-element vector for positive and negative velocity

link.dh link.dyn link.qlim link.islimit(q) link.offset

r+a r+a r+a r r+a

row of legacy DH matrix row of legacy DYN matrix joint coordinate limits, 2-vector return true if value of q is outside the joint limit bounds joint coordinate offset (see discussion for robot object).

The default is for standard Denavit-Hartenberg conventions, zero friction, mass and inertias. The display method gives a one-line summary of the link’s kinematic parameters. The show method displays as many link parameters as have been initialized for that link.

Examples
>> L = link([-pi/2, 0.02, 0, 0.15]) L = -1.570796 0.020000 0.000000 >> L.RP ans = R >> L.mdh ans = 0 >> L.G = 100; >> L.Tc = 5; >> L

0.150000

R

(std)

Robotics Toolbox Release 7.1

Peter Corke, April 2002

link

31

L = -1.570796 0.020000 >> show(L) alpha = -1.5708 A = 0.02 theta = 0 D = 0.15 sigma = 0 mdh = 0 G = 100 Tc = 5 -5 >>

0.000000

0.150000

R

(std)

Algorithm

For the standard Denavit-Hartenberg conventions the homogeneous transform Ai = cos "i sin "i 0 0 sin "i cos !i cos "i cos !i sin !i 0 sin "i sin !i cos "i sin !i cos !i 0 ai cos "i ai sin "i di 1

i1

represents each link’s coordinate frame with respect to the previous link’s coordinate system. For a revolute joint "i is offset by For the modied Denavit-Hartenberg conventions it is instead Ai = cos "i sin "i cos !i1 sin "i sin !i1 0 sin "i cos "i cos !i1 cos "i sin !i1 0 0 sin !i1 cos !i1 0 ai1 di sin !i1 di cos !i1 1

i1

See Also References

showlink, robot

R. P. Paul. Robot Manipulators: Mathematics, Programming, and Control. MIT Press, Cambridge, Massachusetts, 1981. J. J. Craig, Introduction to Robotics. Addison Wesley, second ed., 1989.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

maniplty

32

maniplty
Purpose Synopsis
Manipulability measure

m = maniplty(robot, q) m = maniplty(robot, q, which)

Description

maniplty computes the scalar manipulability index for the manipulator at the given pose. Manipulability varies from 0 (bad) to 1 (good). robot is a robot object that contains kinematic and optionally dynamic parameters for the manipulator. Two measures are supported and are selected by the optional argument which can be either ’yoshikawa’ (default) or ’asada’. Yoshikawa’s manipulability measure is based purely on kinematic data, and gives an indication of how ‘far’ the manipulator is from singularities and thus able to move and exert forces uniformly in all directions. Asada’s manipulability measure utilizes manipulator dynamic data, and indicates how close the inertia ellipsoid is to spherical. If q is a vector maniplty returns a scalar manipulability index. If q is a matrix maniplty returns a column vector and each row is the manipulability index for the pose specied by the corresponding row of q.

Algorithm

Yoshikawa’s measure is based on the condition number of the manipulator Jacobian )yoshi = |J(q)J(q) |

Asada’s measure is computed from the Cartesian inertia matrix M(x) = J(q)T M(q)J(q)1 The Cartesian manipulator inertia ellipsoid is x M(x)x = 1 and gives an indication of how well the manipulator can accelerate in each of the Cartesian directions. The scalar measure computed here is the ratio of the smallest/largest ellipsoid axes )asada = min x max x

Ideally the ellipsoid would be spherical, giving a ratio of 1, but in practice will be less than 1.

See Also References

jacob0, inertia,robot

T. Yoshikawa, “Analysis and control of robot manipulators with redundancy,” in Proc. 1st Int. Symp. Robotics Research, (Bretton Woods, NH), pp. 735–747, 1983.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

robot/nofriction

33

robot/nofriction
Purpose Synopsis
Remove friction from robot object

robot2 = nofriction(robot) robot2 = nofriction(robot, ’all’)

Description

Return a new robot object with modied joint friction properties. The rst form sets the Coulomb friction values to zero in the constituent links The second form sets viscous and Coulomb friction values in the constituent links are set to zero. The resulting robot object has its name string prepended with ’NF/’. This is important for forward dynamics computation (fdyn()) where the presence of friction can prevent the numerical integration from converging.

See Also

link/nofriction, robot, link, friction, fdyn

Robotics Toolbox Release 7.1

Peter Corke, April 2002

link/nofriction

34

link/nofriction
Purpose Synopsis
Remove friction from link object

link2 = nofriction(link) link2 = nofriction(link, ’all’)

Description

Return a new link object with modied joint friction properties. The rst form sets the Coulomb friction values to zero. The second form sets both viscous and Coulomb friction values to zero. This is important for forward dynamics computation (fdyn()) where the presence of friction can prevent the numerical integration from converging.

See Also

robot/nofriction, link, friction, fdyn

Robotics Toolbox Release 7.1

Peter Corke, April 2002

oa2tr

35

oa2tr
Purpose Synopsis Description
Convert OA vectors to homogeneous transform

oa2tr(o, a)

oa2tr returns a rotational homogeneous transformation specied in terms of the Cartesian orientation and approach vectors o and a respectively.

Algorithm
T= o×a 0 o 0 a 0 0 1

where o and a are unit vectors corresponding to o and a respectively.

See Also

rpy2tr, eul2tr

Robotics Toolbox Release 7.1

Peter Corke, April 2002

perturb

36

perturb
Purpose Synopsis Description
Perturb robot dynamic parameters

robot2 = perturb(robot, p)

Return a new robot object with randomly modied dynamic parameters: link mass and inertia. The perturbation is multiplicative so that values are multiplied by random numbers in the interval (1-p) to (1+p). Useful for investigating the robustness of various model-based control schemes where one model forms the basis of the model-based controller and the peturbed model is used for the actual plant. The resulting robot object has its name string prepended with ’P/’.

See Also

fdyn, rne, robot

Robotics Toolbox Release 7.1

Peter Corke, April 2002

puma560

37

puma560
Purpose Synopsis Description
Create a Puma 560 robot object

puma560

Creates the robot object p560 which describes the kinematic and dynamic characteristics of a Unimation Puma 560 manipulator. The kinematic conventions used are as per Paul and Zhang, and all quantities are in standard SI units. Also denes the joint coordinate vectors qz, qr and qstretch corresponding to the zero-angle, ready and fully extended (in X-direction) poses respectively.

Details of coordinate frames used for the Puma 560 shown here in its zero angle pose.

See Also References

robot, puma560akb, stanford

R. P. Paul and H. Zhang, “Computationally efcient kinematics for manipulators with spherical wrists,” Int. J. Robot. Res., vol. 5, no. 2, pp. 32–44, 1986. P. Corke and B. Armstrong-H louvry, “A search for consensus among model parameters reported for e the PUMA 560 robot,” in Proc. IEEE Int. Conf. Robotics and Automation, (San Diego), pp. 1608– 1613, May 1994. P. Corke and B. Armstrong-H louvry, “A meta-study of PUMA 560 dynamics: A critical appraisal of e literature data,” Robotica, vol. 13, no. 3, pp. 253–258, 1995.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

puma560akb

38

puma560akb
Purpose Synopsis Description
Create a Puma 560 robot object

puma560akb

Creates the robot object p560m which describes the kinematic and dynamic characteristics of a Unimation Puma 560 manipulator. It uses Craig’s modied Denavit-Hartenberg notation with the particular kinematic conventions from Armstrong, Khatib and Burdick. All quantities are in standard SI units. Also denes the joint coordinate vectors qz, qr and qstretch corresponding to the zero-angle, ready and fully extended (in X-direction) poses respectively.

See Also References

robot, puma560, stanford

B. Armstrong, O. Khatib, and J. Burdick, “The explicit dynamic model and inertial parameters of the Puma 560 arm,” in Proc. IEEE Int. Conf. Robotics and Automation, vol. 1, (Washington, USA), pp. 510–18, 1986.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

qinterp

39

qinterp
Purpose Synopsis Description
Interpolate unit-quaternions

QI = qinterp(Q1, Q2, r)

Return a unit-quaternion that interpolates between Q1 and Q2 as r varies between 0 and 1 inclusively. This is a spherical linear interpolation (slerp) that can be interpreted as interpolation along a great circle arc on a sphere. If r is a vector, then a cell array of quaternions is returned corresponding to successive values of r.

Examples

A simple example >> q1 = quaternion(rotx(0.3)) q1 = 0.98877 <0.14944, 0, 0> >> q2 = quaternion(roty(-0.5)) q2 = 0.96891 <0, -0.2474, 0> >> qinterp(q1, q2, 0) ans = 0.98877 <0.14944, 0, 0> >> qinterp(q1, q2, 1) ans = 0.96891 <0, -0.2474, 0> >> qinterp(q1, q2, 0.3) ans = 0.99159 <0.10536, -0.075182, 0> >>

References

K. Shoemake, “Animating rotation with quaternion curves.,” in Proceedings of ACM SIGGRAPH, (San Francisco), pp. 245–254, The Singer Company, Link Flight Simulator Division, 1985.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

quaternion

40

quaternion
Purpose Synopsis
Quaternion object

q q q q

= = = =

quaternion(qq) quaternion(v, theta) quaternion(R) quaternion([s vx vy vz])

Description

quaternion is the constructor for a quaternion object. The rst form returns a new object with the same value as its argument. The second form initializes the quaternion to a rotation of theta about the vector v.

Examples

A simple example >> quaternion(1, [1 0 0]) ans = 0.87758 <0.47943, 0, 0> >> quaternion( rotx(1) ) ans = 0.87758 <0.47943, 0, 0> >> The third form sets the quaternion to a rotation equivalent to the given 3 × 3 rotation matrix, or the rotation submatrix of a 4 × 4 homogeneous transform. The fourth form sets the four quaternion elements directly where s is the scalar component and [vx vy vz] the vector. All forms, except the last, return a unit quaternion, ie. one whose magnitude is unity. Some operators are overloaded for the quaternion class

Robotics Toolbox Release 7.1

Peter Corke, April 2002

quaternion

41

q1 * q2 q * v q1 / q2 q∧j

double(q) inv(q) norm(q) plot(q) unit(q)

returns quaternion product or compounding returns a quaternion vector product, that is the vector v is rotated by the quaternion. v is a 3 × 3 vector returns q1 q1 2 returns q j where j is an integer exponent. For j > 0 the result is obtained by repeated multiplication. For j < 0 the nal result is inverted. returns the quaternion coefents as a 4-element row vector returns the quaterion inverse returns the quaterion magnitude displays a 3D plot showing the standard coordinate frame after rotation by q. returns the corresponding unit quaterion

Some public class variables methods are also available for reference only. method quaternion.d quaternion.s quaternion.v quaternion.t quaternion.r Returns return 4-vector of quaternion elements return scalar component return vector component return equivalent homogeneous transformation matrix return equivalent orthonormal rotation matrix

Examples
>> t = rotx(0.2) t = 1.0000 0 0 0 0.9801 -0.1987 0 0.1987 0.9801 0 0 0 >> q1 = quaternion(t) q1 = 0.995 <0.099833, 0, 0> >> q1.r ans = 1.0000 0 0

0 0 0 1.0000

0 0.9801 0.1987

0 -0.1987 0.9801

>> q2 = quaternion( roty(0.3) ) q2 = 0.98877 <0, 0.14944, 0> >> q1 * q2 ans =

Robotics Toolbox Release 7.1

Peter Corke, April 2002

quaternion

42

0.98383 <0.098712, 0.14869, 0.014919> >> q1*q1 ans = 0.98007 <0.19867, 0, 0> >> q12 ans = 0.98007 <0.19867, 0, 0> >> q1*inv(q1) ans = 1 <0, 0, 0> >> q1/q1 ans = 1 <0, 0, 0> >> q1/q2 ans = 0.98383 <0.098712, -0.14869, -0.014919> >> q1*q2-1 ans = 0.98383 <0.098712, -0.14869, -0.014919>

Cautionary

At the moment vectors or arrays of quaternions are not supported. You can however use cell arrays to hold a number of quaternions.

See Also References

quaternion/plot

K. Shoemake, “Animating rotation with quaternion curves.,” in Proceedings of ACM SIGGRAPH, (San Francisco), pp. 245–254, The Singer Company, Link Flight Simulator Division, 1985.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

quaternion/plot

43

quaternion/plot
Purpose Synopsis Description
Plot quaternion rotation

plot(Q)

plot is overloaded for quaternion objects and displays a 3D plot which shows how the standard axes are transformed under that rotation.

Examples

A rotation of 0.3rad about the X axis. Clearly the X axis is invariant under this rotation. >> q=quaternion(rotx(0.3)) q = 0.85303<0.52185, 0, 0> >> plot(q)

1 Y 0.5 Z X

Z

0

!0.5

!1 1 0.5 0 0 !0.5 Y !1 !1 !0.5 X 0.5 1

See Also

quaternion

Robotics Toolbox Release 7.1

Peter Corke, April 2002

rne

44

rne
Purpose Synopsis
Compute inverse dynamics via recursive Newton-Euler formulation

tau = rne(robot, q, qd, qdd) tau = rne(robot, [q qd qdd]) tau = rne(robot, q, qd, qdd, grav) tau = rne(robot, [q qd qdd], grav) tau = rne(robot, q, qd, qdd, grav, fext) tau = rne(robot, [q qd qdd], grav, fext)

Description

rne computes the equations of motion in an efcient manner, giving joint torque as a function of joint position, velocity and acceleration. If q, qd and qdd are row vectors then tau is a row vector of joint torques. If q, qd and qdd are matrices then tau is a matrix in which each row is the joint torque for the corresponding rows of q, qd and qdd. Gravity direction is dened by the robot object but may be overridden by providing a gravity acceleration vector grav = [gx gy gz]. An external force/moment acting on the end of the manipulator may also be specied by a 6-element vector fext = [Fx Fy Fz Mx My Mz] in the end-effector coordinate frame. The torque computed may contain contributions due to armature inertia and joint friction if these are specied in the parameter matrix dyn. The MEX-le version of this function is over 1000 times faster than the M-le. See Section 1 of this manual for information about how to compile and install the MEX-le.

Algorithm

Coumputes the joint torque q ˙ ˙ ˙ # = M(q)¨ + C(q, q)q + F(q) + G(q) where M is the manipulator inertia matrix, C is the Coriolis and centripetal torque, F the viscous and Coulomb friction, and G the gravity load.

Cautionary See Also Limitations

The MEX le currently ignores support base and tool transforms.

robot, fdyn, accel, gravload, inertia, friction

A MEX le is currently only available for Sparc architecture.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

rne

45

References

J. Y. S. Luh, M. W. Walker, and R. P. C. Paul. On-line computational scheme for mechanical manipulators. ASME Journal of Dynamic Systems, Measurement and Control, 102:69–76, 1980.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

robot

46

robot
Purpose Synopsis
Robot object

r r r r r

= = = = =

robot robot(rr) robot(link ...) robot(DH ...) robot(DYN ...)

Description

robot is the constructor for a robot object. The rst form creates a default robot, and the second form returns a new robot object with the same value as its argument. The third form creates a robot from a cell array of link objects which dene the robot’s kinematics and optionally dynamics. The fourth and fth forms create a robot object from legacy DH and DYN format matrices. The last three forms all accept optional trailing string arguments which are taken in order as being robot name, manufacturer and comment. Since Matlab does not support the concept of public class variables methods have been written to allow robot object parameters to be referenced (r) or assigned (a) as given by the following table method robot.n robot.link robot.name robot.manuf robot.comment robot.gravity robot.mdh robot.base robot.tool robot.dh robot.dyn robot.q robot.qlim robot.islimit Operation r r+a r+a r+a r+a r+a r r+a r+a r r r+a r+a r Returns number of joints cell array of link objects robot name string robot manufacturer string general comment string 3-element vector dening gravity direction DH convention: 0 if standard, 1 if modied. Determined from the link objects. homogeneous transform dening base of robot homogeneous transform dening tool of robot legacy DH matrix legacy DYN matrix joint coordinates joint coordinate limits, n × 2 matrix joint limit vector, for each joint set to -1, 0 or 1 depending if below low limit, OK, or greater than upper limit joint coordinate offsets options for plot() line style for robot graphical links line style for robot shadow links graphics handles

robot.offset robot.plotopt robot.lineopt robot.shadowopt robot.handle

r+a r+a r+a r+a r+a

Some of these operations at the robot level are actually wrappers around similarly named link object

Robotics Toolbox Release 7.1

Peter Corke, April 2002

robot

47

functions: offset, qlim, islimit. The offset vector is added to the user specied joint angles before any kinematic or dynamic function is invoked (it is actually implemented within the link object). Similarly it is subtracted after an operation such as inverse kinematics. The need for a joint offset vector arises because of the constraints of the Denavit-Hartenberg (or modied Denavit-Hartenberg) notation. The pose of the robot with zero joint angles is frequently some rather unusual (or even unachievable) pose. The joint coordinate offset provides a means to make an arbitrary pose correspond to the zero joint angle case. Default values for robot parameters are: robot.name robot.manuf robot.comment robot.gravity robot.offset robot.base robot.tool robot.lineopt robot.shadowopt ’noname’ ” ” [0 0 9.81] m/s2 ones(n,1) eye(4,4) eye(4,4) ’Color’, ’black’, ’Linewidth’, 4 ’Color’, ’black’, ’Linewidth’, 1

The multiplication operator, *, is overloaded and the product of two robot objects is a robot which is the series connection of the multiplicands. Tool transforms of all but the last robot are ignored, base transform of all but the rst robot are ignored. The plot function is also overloaded and is used to provide a robot animation.

Examples
>> L{1} = link([ pi/2 L = [1x1 link] 0 0 0])

>> L{2} = link([ 0 0 0.5 0]) L = [1x1 link] [1x1 link] >> r = robot(L) r = (2 axis, RR) grav = [0.00 0.00 9.81] standard D&H parameters alpha 1.570796 0.000000 A 0.000000 0.000000 theta 0.000000 0.500000 D 0.000000 0.000000 R/P R R

(std) (std)

Robotics Toolbox Release 7.1

Peter Corke, April 2002

robot

48

>>

See Also

link,plot

Robotics Toolbox Release 7.1

Peter Corke, April 2002

robot/plot

49

robot/plot
Purpose Synopsis
Graphical robot animation

plot(robot, q) plot(robot, q, arguments...)

y

z x

Puma 560

0.8 0.6

Description

plot is overloaded for robot objects and displays a graphical representation of the robot given the kinematic information in robot. The robot is represented by a simple stick gure polyline where line segments join the origins of the link coordinate frames. If q is a matrix representing a joint-space trajectory then an animation of the robot motion is shown.

GRAPHICAL ANNOTATIONS
The basic stick gure robot can be annotated with shadow on the ‘oor’ XYZ wrist axes and labels, shown by 3 short orthogonal line segments which are colored: red (X or normal), green (Y or orientation) and blue (Z or approach). They can be optionally labelled XYZ or NOA. joints, these are 3D cylinders for revolute joints and boxes for prismatic joints the robot’s name All of these require some kind of dimension and this is determined using a simple heuristic from

Robotics Toolbox Release 7.1

Peter Corke, April 2002

robot/plot

50

the workspace dimensions. This dimension can be changed by setting the multiplicative scale factor using the mag option below. These various annotations do slow the rate at which animations will be rendered.

OPTIONS
Options are specied by a variable length argument list comprising strings and numeric values. The allowed values are:

workspace w perspective ortho base, nobase wrist, nowrist name, noname shadow, noshadow joints, nojoints xyz noa mag scale erase, noerase loop, noloop

set the 3D plot bounds or workspace to the matrix [xmin xmax ymin ymax zmin zmax] show a perspective view show an orthogonal view control display of base, a line from the oor upto joint 0 control display of wrist axes control display of robot name near joint 0 control display of a ’shadow’ on the oor control display of joints, these are cylinders for revolute joints and boxes for prismatic joints wrist axis labels are X, Y, Z wrist axis labels are N, O, A annotation scale factor control erasure of robot after each change control whether animation is repeated endlessly

The options come from 3 sources and are processed in the order: 1. Cell array of options returned by the function PLOTBOTOPT if found on the user’s current path. 2. Cell array of options returned by the .plotopt method of the robot object. These are set by the .plotopt method. 3. List of arguments in the command line.

GETTING GRAPHICAL ROBOT STATE
Each graphical robot has a unique tag set equal to the robot’s name. When plot is called it looks for all graphical objects with that name and moves them. The graphical robot holds a copy of the robot object as UserData. That copy contains the graphical handles of all the graphical sub-elements of the robot and also the current joint angle state. This state is used, and adjusted, by the drivebot function. The current joint angle state can be obtained by q = plot(robot). If multiple instances exist, that of the rst one returned by findobj() is given.

Examples

To animate two Pumas moving in the same gure window. >> >> >> >> clf p560b = p560; p560b.name = ’Another Puma 560’; p560b.base = transl([-.05 0.5 0]);

% duplicate the robot % give it a unique name % move its base

Robotics Toolbox Release 7.1

Peter Corke, April 2002

robot/plot

51

>> >> >> >> >> >> >> >> >>

plot(p560, qr); % display it at ready position hold on plot(p560b, qr); % display it at ready position t = [0:0.056:10]; jt = jtraj(qr, qstretch, t); % trajectory to stretch pose for q = jt’, % for all points on the path plot(p560, q); plot(p560b, q); end

To show multiple views of the same robot. >> >> >> >> >> >> clf figure plot(p560, qz); figure plot(p560, qz); plot(p560, qr);

% % % % %

create a new figure add a graphical robot create another figure add a graphical robot both robots should move

Now the two gures can be adjusted to give different viewpoints, for instance, plan and elevation.

Cautionary

plot() options are only processed on the rst call when the graphical object is established, they are skipped on subsequent calls. Thus if you wish to change options, clear the gure before replotting.

See Also

drivebot, fkine, robot

Robotics Toolbox Release 7.1

Peter Corke, April 2002

rotvec

52

rotvec
Purpose Synopsis Description
Rotation about a vector

T = rotvec(v, theta)

rotvec returns a homogeneous transformation representing a rotation of theta radians about the vector v.

See Also

rotx, roty, rotz

Robotics Toolbox Release 7.1

Peter Corke, April 2002

rotx,roty,rotz

53

rotx,roty,rotz
Purpose Synopsis
Rotation about X, Y or Z axis

T = rotx(theta) T = roty(theta) T = rotz(theta)

Description

Return a homogeneous transformation representing a rotation of theta radians about the X, Y or Z axes.

See Also

rotvec

Robotics Toolbox Release 7.1

Peter Corke, April 2002

rpy2tr

54

rpy2tr
Purpose Synopsis
Roll/pitch/yaw angles to homogeneous transform

T = rpy2tr([r p y]) T = rpy2tr(r,p,y)

Description

rpy2tr returns a homogeneous transformation for the specied roll/pitch/yaw angles in radians. T = RZ (r)RY (p)RX (y)

See Also References

tr2rpy, eul2tr

R. P. Paul, Robot Manipulators: Mathematics, Programming, and Control. sachusetts: MIT Press, 1981.

Cambridge, Mas-

Robotics Toolbox Release 7.1

Peter Corke, April 2002

rtdemo

55

rtdemo
Purpose Synopsis Description Cautionary
Robot Toolbox demonstration

rtdemo

This script provides demonstrations for most functions within the Robotics Toolbox.

This script clears all variables in the workspace and deletes all gures.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

showlink

56

showlink
Purpose Synopsis
Show robot link details

showlink(robot) showlink(link)

Description

Displays in detail all the parameters, including all dened inertial parameters, of a link. The rst form provides this level of detail for all links in the specied manipulator. roll/pitch/yaw angles in radians.

Examples

To show details of Puma link 2 >> showlink(p560.link{2}) alpha = 0 A = 0.4318 theta = 0 D = 0 sigma = 0 mdh = 0 offset = 0 m = 17.4 r = -0.3638 0.006 0.2275 I = 0.13 0 0 0.524 0 0 Jm = 0.0002 G = 107.815 B = 0.000817 Tc = 0.126 -0.071 qlim = >>

0 0 0.539

Robotics Toolbox Release 7.1

Peter Corke, April 2002

stanford

57

stanford
Purpose Synopsis
Create a Stanford manipulator robot object

stanford

2

1

Z

0 y z !1 x Stanford arm

!2 2 1 0 !1 Y !2 !2 !1 X 1 0 2

Description

Creates the robot object stan which describes the kinematic and dynamic characteristics of a Stanford manipulator. Species armature inertia and gear ratios. All quantities are in standard SI units.

See Also References

robot, puma560

R. Paul, “Modeling, trajectory calculation and servoing of a computer controlled arm,” Tech. Rep. AIM-177, Stanford University, Articial Intelligence Laboratory, 1972. R. P. Paul, Robot Manipulators: Mathematics, Programming, and Control. sachusetts: MIT Press, 1981. Cambridge, Mas-

Robotics Toolbox Release 7.1

Peter Corke, April 2002

tr2diff

58

tr2diff
Purpose Synopsis
Convert a homogeneous transform to a differential motion vector

d = tr2diff(T) d = tr2diff(T1, T2)

Description

The rst form of tr2diff returns a 6-element differential motion vector representing the incremental translation and rotation described by the homogeneous transform T. It is assumed that T is of the form 0 *z *y 0 *z 0 *x 0 *y *x 0 0 dx dy dz 0

The translational elements of d are assigned directly. The rotational elements are computed from the mean of the two values that appear in the skew-symmetric matrix. The second form of tr2diff returns a 6-element differential motion vector representing the displacement from T1 to T2, that is, T2 - T1. d= p2 p1 1/2 (n1 × n2 + o1 × o2 + a1 × a2 )

See Also References

diff2tr

R. P. Paul, Robot Manipulators: Mathematics, Programming, and Control. sachusetts: MIT Press, 1981.

Cambridge, Mas-

Robotics Toolbox Release 7.1

Peter Corke, April 2002

tr2eul

59

tr2eul
Purpose Synopsis Description
Convert a homogeneous transform to Euler angles

[a b c] = tr2eul(T)

tr2eul returns a vector of Euler angles, in radians, corresponding to the rotational part of the homogeneous transform T. Trot = RZ (a)RY (b)RZ (c)

Cautionary

Note that 12 different Euler angle sets or conventions exist. The convention used here is the common one for robotics, but is not the one used for example in the aerospace community.

See Also References

eul2tr, tr2rpy

R. P. Paul, Robot Manipulators: Mathematics, Programming, and Control. sachusetts: MIT Press, 1981.

Cambridge, Mas-

Robotics Toolbox Release 7.1

Peter Corke, April 2002

tr2jac

60

tr2jac
Purpose Synopsis Description
Compute a Jacobian to map differential motion between frames

jac = tr2jac(T)

tr2jac returns a 6 × 6 Jacobian matrix to map differential motions or velocities between frames related by the homogeneous transform T. If T represents a homogeneous transformation from frame A to frame B, A TB , then
B

x = B JA A x ˙ ˙

where B JA is given by tr2jac(T).

See Also References

tr2diff, diff2tr

R. P. Paul, Robot Manipulators: Mathematics, Programming, and Control. sachusetts: MIT Press, 1981.

Cambridge, Mas-

Robotics Toolbox Release 7.1

Peter Corke, April 2002

tr2rpy

61

tr2rpy
Purpose Synopsis Description
Convert a homogeneous transform to roll/pitch/yaw angles

[a b c] = tr2rpy(T)

tr2rpy returns a vector of roll/pitch/yaw angles, in radians, corresponding to the rotational part of the homogeneous transform T Trot = RZ (r)RY (p)RX (y)

See Also References

rpy2tr, tr2eul

R. P. Paul, Robot Manipulators: Mathematics, Programming, and Control. sachusetts: MIT Press, 1981.

Cambridge, Mas-

Robotics Toolbox Release 7.1

Peter Corke, April 2002

transl

62

transl
Purpose Synopsis
Translational transformation

T = T = v = xyz

transl(x, y, z) transl(v) transl(T) = transl(TC)

Description

The rst two forms return a homogeneous transformation representing a translation expressed as three scalar x, y and z, or a Cartesian vector v. The third form returns the translational part of a homogeneous transform as a 3-element column vector. The fourth form returns a matrix whose columns are the X, Y and Z columns of the 4×4×m Cartesian trajectory matrix TC.

See Also

ctraj

Robotics Toolbox Release 7.1

Peter Corke, April 2002

trinterp

63

trinterp
Purpose Synopsis Description
Interpolate homogeneous transforms

T = trinterp(T0, T1, r)

trinterp interpolates between the two homogeneous transforms T0 and T1 as r varies between 0 and 1 inclusively. This is generally used for computing straight line or ‘Cartesian’ motion. Rotational interpolation is achieved using quaternion spherical linear interpolation.

Examples

Interpolation of homogeneous transformations. >> t1=rotx(.2) t1 = 1.0000 0 0 0

0 0.9801 0.1987 0

0 -0.1987 0.9801 0

0 0 0 1.0000

>> t2=transl(1,4,5)*roty(0.3) t2 = 0.9553 0 -0.2955 0

0 1.0000 0 0

0.2955 0 0.9553 0

1.0000 4.0000 5.0000 1.0000

>> trinterp(t1,t2,0) % should be t1 ans = 1.0000 0 0 0

0 0.9801 0.1987 0

0 -0.1987 0.9801 0

0 0 0 1.0000

>> trinterp(t1,t2,1) ans = 0.9553 0 -0.2955 0

% should be t2

0 1.0000 0 0

0.2955 0 0.9553 0

1.0000 4.0000 5.0000 1.0000

Robotics Toolbox Release 7.1

Peter Corke, April 2002

trinterp

64

>> trinterp(t1,t2,0.5) % ’half way’ in between ans = 0.9887 0.0075 -0.1494 0 >>

0.0075 0.9950 0.0998 0

0.1494 -0.0998 0.9837 0

0.5000 2.0000 2.5000 1.0000

See Also References

ctraj, qinterp

R. P. Paul, Robot Manipulators: Mathematics, Programming, and Control. sachusetts: MIT Press, 1981.

Cambridge, Mas-

Robotics Toolbox Release 7.1

Peter Corke, April 2002

trnorm

65

trnorm
Purpose Synopsis Description
Normalize a homogeneous transformation

TN = trnorm(T)

Returns a normalized copy of the homogeneous transformation T. Finite word length arithmetic can lead to homogeneous transformations in which the rotational submatrix is not orthogonal, that is, det(R) = 1. Normalization is performed by orthogonalizing the rotation submatrix n = o × a. oa2tr

Algorithm See Also References

J. Funda, “Quaternions and homogeneous transforms in robotics,” Master’s thesis, University of Pennsylvania, Apr. 1988.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

twolink

66

twolink
Purpose Synopsis
Load kinematic and dynamic data for a simple 2-link mechanism

twolink

2

1 yz x

Z

0 Simple two link !1

!2 2 1 0 !1 Y !2 !2 !1 X 1 0 2

Description

Creates the robot object tl which describes the kinematic and dynamic characteristics of a simple two-link planar manipulator. The manipulator operates in the horizontal (XY) plane and is therefore not inuenced by gravity. Mass is assumed to be concentrated at the joints. All masses and lengths are unity.

See Also References

puma560, stanford

Fig 3-6 of “Robot Dynamics and Control” by M.W. Spong and M. Vidyasagar, 1989.

Robotics Toolbox Release 7.1

Peter Corke, April 2002

unit

67

unit
Purpose Synopsis Description Algorithm
vn = Unitize a vector

vn = unit(v)

unit returns a unit vector aligned with v.

v ||v||

Robotics Toolbox Release 7.1

Peter Corke, April 2002

dh (legacy)

68

dh (legacy)
Purpose Description
Matrix representation of manipulator kinematics

A dh matrix describes the kinematics of a manipulator in a general way using the standard DenavitHartenberg conventions. Each row represents one link of the manipulator and the columns are assigned according to the following table. Column 1 2 3 4 5 Symbol !i Ai "i Di (i Description link twist angle link length link rotation angle link offset distance joint type; 0 for revolute, non-zero for prismatic

If the last column is not given, toolbox functions assume that the manipulator is all-revolute. For an n-axis manipulator dh is an n × 4 or n × 5 matrix. The rst 5 columns of a dyn matrix contain the kinematic parameters and maybe used anywhere that a dh kinematic matrix is required — the dynamic data is ignored.

Lengths Ai and Di may be expressed in any unit, and this choice will ow on to the units in which homogeneous transforms and Jacobians are represented. All angles are in radians.

See Also References

puma560,stanford,mdh

R. P. Paul, Robot Manipulators: Mathematics, Programming, and Control. sachusetts: MIT Press, 1981.

Cambridge, Mas-

Robotics Toolbox Release 7.1

Peter Corke, April 2002

dyn (legacy)

69

dyn (legacy)
Purpose Description
Matrix representation of manipulator kinematics and dynamics

A dyn matrix describes the kinematics and dynamics of a manipulator in a general way using the standard Denavit-Hartenberg conventions. Each row represents one link of the manipulator and the columns are assigned according to the following table. Column 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 Symbol ! A " D ( mass rx ry rz Ixx Iyy Izz Ixy Iyz Ixz Jm G B Tc+ TcDescription link twist angle link length link rotation angle link offset distance joint type; 0 for revolute, non-zero for prismatic mass of the link link COG with respect to the link coordinate frame

elements of link inertia tensor about the link COG

armature inertia reduction gear ratio; joint speed/link speed viscous friction, motor referred coulomb friction (positive rotation), motor referred coulomb friction (negative rotation), motor referred

For an n-axis manipulator, dyn is an n × 20 matrix. The rst 5 columns of a dyn matrix contain the kinematic parameters and maybe used anywhere that a dh kinematic matrix is required — the dynamic data is ignored.

All angles are in radians. The choice of all other units is up to the user, and this choice will ow on to the units in which homogeneous transforms, Jacobians, inertias and torques are represented.

See Also

dh

Robotics Toolbox Release 7.1

Peter Corke, April 2002


相关文章:
robotics toolbox for matlab的机器人仿真
%用 DYN 矩阵来创建一个机器人对象 利用 MATLAB 中 Robotics Toolbox 工具箱中的 transl、rotx、roty 和 rotz 可以 实现用齐次变换矩阵表示平移变换和旋转变换。...
matlab中robotics toolbox的函数解说
PUMA560 的 MATLAB 仿真 要建立 PUMA560 的机器人对象,首先我们要了解 PUMA560 的 D-H 参数,之后我们可以利 用 Robotics Toolbox 工具箱中的 link 和 robot ...
基于MATLAB Robotics Tools的机械臂仿真
基于MATLAB Robotics Tools 的机械臂仿真 【摘要】在 MATLAB 环境下,对 puma560 机器人进行运动学仿真研究,利用 Robotics Toolbox 工具箱编制了简单的程序语句,建立...
matlab中robotics_toolbox的函数解说
PUMA560 的 MATLAB 仿真 要建立 PUMA560 的机器人对象,首先我们要了解 PUMA560 的 D-H 参数,之后我们可以利 用 Robotics Toolbox 工具箱中的 link 和 robot ...
如何在matlab2014a中添加Robotics Toolbox for matlab工具箱
暂无评价|0人阅读|0次下载|举报文档如何在matlab2014a中添加Robotics Toolbox for matlab工具箱_电子/电路_工程科技_专业资料。Robotics Toolbox for matlab工具箱 如...
matlab中robotics toolbox的应用
PUMA560 的 MATLAB 仿真 要建立 PUMA560 的机器人对象,首先我们要了解 PUMA560 的 D-H 参数,之后我们可以利用 Robotics Toolbox 工具箱中的 link 和 robot ...
《机器人技术》实验报告
(3) 在机器人工具箱 Robotics Toolbox 中,应用 Link 函数、robot 函数构建机 器人对象。 (详细了解 link, robot 函数的调用格式) 答:link 程序如下: L1 = ...
matlab robotics toolbox demo
matlab robotics toolbox demo_计算机软件及应用_IT/计算机_专业资料。robottool demoMATLAB ROBOTTOOL rtdemo 演示目录一、rtdemo 机器人工具箱演示... 2 二、tran...
Matlab robot tool 学习
PUMA560 的 MATLAB 仿真 要建立 PUMA560 的机器人对象,首先我们要了解 PUMA560 的 D-H 参数, 之后我们可以利用 Robotics Toolbox 工具箱中的 link 和 robot ...
更多相关标签:
robotics toolbox下载 | robotics toolbox | robotics toolbox教程 | robotics toolbox实例 | robotics toolbox安装 | robotics toolbox使用 | robotics toolbox9.10 | robotics toolbox 8.0 |