robotics toolbox - computer engineering group - · pdf filefigure 4: robotics toolbox example...

RoboticsTOOLBOXfor MATLAB

(Release7)

−1

−0.5

0

0.5

1

−1

−0.8

−0.6

−0.4

−0.2

0

0.2

0.4

0.6

0.8

1

−1

−0.8

−0.6

−0.4

−0.2

0

0.2

0.4

0.6

0.8

1

Puma 560

xyz

−4−2

02

4

−4

−2

0

2

42

2.5

3

3.5

4

4.5

5

5.5

q2q3

I11

PeterI. [email protected]

April 2002http://www.cat.csiro.au/cmst/staff/pic/robot

PeterI. CorkeCSIROManufacturingScienceandTechnologyPullenvale,AUSTRALIA, 4069.2002http://www.cat.csiro.au/cmst/staff/pic/robot

c 2002by PeterI. Corke.

3

1Preface

1 Intr oduction

This, theseventhreleaseof theToolbox,representsnearlya decadeof tinkeringanda sub-stantiallevel of maturity. It finally includesmany of thefeaturesI’ vebeenplanningto addfor sometime,particularlyMEX files,Simulink supportandmodifiedDenavit-Hartenbergsupport. The previous releasehashad thousandsof downloadsand the mailing list hashundredsof subscribers.

The Toolbox providesmany functionsthat areuseful in roboticsincluding suchthingsaskinematics,dynamics,andtrajectorygeneration.The Toolbox is useful for simulationaswell asanalyzingresultsfrom experimentswith realrobots.

TheToolboxis basedon a very generalmethodof representingthekinematicsanddynam-icsof serial-linkmanipulators.Theseparametersareencapsulatedin Matlabobjects.Robotobjectscanbecreatedby theuserfor any serial-linkmanipulatorandanumberof examplesareprovidedfor well know robotssuchasthePuma560andtheStanfordarm.TheToolboxalso provides functionsfor manipulatingand converting betweendatatypessuchas vec-tors,homogeneoustransformationsandunit-quaternionswhich arenecessaryto represent3-dimensionalpositionandorientation.

Theroutinesarewritten in a straightforwardmannerwhich allows for easyunderstanding,perhapsat theexpenseof computationalefficiency. My guidein all of this work hasbeenthebookof Paul[1], now out of print, but which I grew up with. If you feel stronglyaboutcomputationalefficiency then you can always rewrite the function to be more efficient,compiletheM-file usingtheMatlabcompiler, or createa MEX version.

1.1 What’s new

This releasehassomesignificantnew functionalityaswell assomebug fixes.

Full supportfor modified(Craig’s)Denavit-Hartenberg notation,forwardandinversekinematics,Jacobiansandforwardandinversedynamics.

Simulinkblocksetlibrary anddemonstrationsincluded,seeSection2

MEX implementationof recursive Newton-Euleralgorithm written in portableC.Speedimprovementsof at least1000. Testedon Solaris,Linux andWindows. SeeSection1.9.

Fixedstill morebugsandmissingfiles in quaternioncode.

Remove ‘@’ notationfrom fdyn to allow operationunderMatlab5 and6.

Fairly major updateof documentationto ensureconsistency betweencode,onlinehelpandthis manual.

1 INTRODUCTION 4

All codeis now underCVScontrolwhichshouldeliminatemany of theversioningproblemsI hadpreviouslydueto developingthecodeacrossmultiple computers.

1.2 Contact

TheToolboxhomepageis at

http://www.cat.csiro.au/cmst/staff/pic/robot

Thispagewill alwayslist thecurrentreleasedversionnumberaswell asbug fixesandnewcodein betweenmajorreleases.

A Mailing List is alsoavailable,subscriptionsdetailsareavailableoff thatwebpage.

1.3 How to obtain the Toolbox

TheRoboticsToolboxis freelyavailablefrom theToolboxhomepageat

http://www.cat.csiro.au/cmst/staff/pic/robot

Thefiles areavailablein eithergzippedtar format(.gz)or zip format(.zip). Thewebpagerequestssomeinformationfrom you regardingsuchasyour country, type of organizationandapplication. This is just a meansfor me to gaugeinterestand to help convince mybosses(andmyself)thatthis is a worthwhileactivity.

The file robot.pdf is a comprehensive manualwith a tutorial introductionand detailsof eachToolbox function. A menu-drivendemonstrationcanbe invoked by the functionrtdemo .

1.4 MATLAB version issues

TheToolboxworkswith MATLAB version6 andgreaterandhasbeentestedon a Sunwithversion6.

The Toolbox doesnot function underMATLAB v3.x or v4.x sincethoseversionsdo notsupportobjects. An older versionof the Toolbox, available from the Matlab4 ftp site isworkablebut lackssomefeaturesof this currentToolboxrelease.

1.5 Acknowledgements

I amgratefulfor thesupportof my employer, CSIRO, for supportingmein thisactivity andproviding mewith theMatlabtoolsandwebserver.

I have correspondedwith a greatmany peoplevia emailsincethefirst releaseof this Tool-box. Somehave identifiedbugsandshortcomingsin the documentation,andeven better,somehaveprovidedbugfixesandevennew modules,thankyou.

1 INTRODUCTION 5

1.6 Support, usein teaching,bug fixes,etc.

I’m alwayshappy to correspondwith peoplewho have foundgenuinebugsor deficienciesin theToolbox,or who have suggestionsaboutwaysto improve its functionality. HoweverI draw theline at providing helpfor peoplewith their assignmentsandhomework!

Many peopleareusingtheToolboxfor teachingandthis is somethingthatI would encour-age.If you planto duplicatethedocumentationfor classusethenevery copy mustincludethefront page.

If youwantto cite theToolboxpleaseuse

@ARTICLECorke96b,

AUTHOR = P.I. Corke,

JOURNAL = IEEE Robotics and Automation Magazine,

MONTH = mar,

NUMBER = 1,

PAGES = 24-32,

TITLE = A Robotics Toolbox for MATLAB,

VOLUME = 3,

YEAR = 1996

which is alsogivenin electronicform in theREADME file.

1.7 A noteon kinematic conventions

Many peoplearenot awarethat therearetwo quitedifferentformsof Denavit-Hartenbergrepresentationfor serial-linkmanipulatorkinematics:

1. Classicalaspertheoriginal 1955paperof Denavit andHartenberg, andusedin text-bookssuchasby Paul[1], Fuetal[2], or SpongandVidyasagar[3].

2. Modified form asintroducedby Craig[4] in his text book.

Bothnotationsrepresenta joint as2 translations(A andD) and2 rotationangles(α andθ).However the expressionsfor the link transformmatricesarequite different. In short,youmustknow which kinematicconventionyourDenavit-Hartenberg parametersconformto.

Unfortunatelymany sourcesin theliteraturedonotspecifythiscrucialpieceof information.Most textbookscover only oneanddo not evenalludeto theexistenceof theother. Theseissuesarediscussedfurtherin Section3.

TheToolboxhasfull supportfor boththeclassicalandmodifiedconventions.

1.8 Creatinga newrobot definition

Let’stakeasimpleexamplelikethetwo-link planarmanipulatorfromSpong& Vidyasagar[3](Figure3-6,p73)which hasthefollowing (standard)Denavit-Hartenberg link parameters

1 INTRODUCTION 6

Link ai αi di θi

1 1 0 0 θ12 1 0 0 θ2

wherewe havesetthelink lengthsto 1. Now we cancreatea pairof 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] standard D&H parameters

alpha A theta D R/P

0.000000 1.000000 0.000000 0.000000 R (std)

0.000000 1.000000 0.000000 0.000000 R (std)

>>

The first few lines createlink objects,oneper robot link. Note the secondargumenttolink whichspecifiesthatthestandardD&H conventionsareto beused(this is actuallythedefault). Theargumentsto thelink objectcanbefoundfrom

>> help link

.

.

LINK([alpha A theta D sigma], CONVENTION)

.

.

which shows the orderin which the link parametersmustbe passed(which is differenttothecolumnorderof the tableabove). Thefifth elementof thefirst argument,sigma , is aflagthatindicateswhetherthejoint is revolute(sigma is zero)or primsmatic(sigma is nonzero).

1 INTRODUCTION 7

−2

−1

0

1

2

−2

−1

0

1

2−2

−1.5

−1

−0.5

0

0.5

1

1.5

2

XY

Z

noname

xy z

Figure1: Simpletwo-link manipulatormodel.

The link objectsarepassedasa cell arrayto the robot() functionwhich createsa robotobjectwhich is in turn passedto many of theotherToolboxfunctions.

Notethatdisplaysof link dataincludethekinematicconventionin bracketson thefar right.(std) for standardform, and(mod) for modifiedform.

Therobotjust createdcanbedisplayedgraphicallyby

>> plot(r, [0 0])

whichwill createtheplot shown in Figure1.

1.9 UsingMEX files

The RoboticsToolbox Release7 includesportableC sourcecodeto generatea MEX fileversionof the rne function.

The MEX file runsupto 500 times fasterthan the interprettedversionrne.m and this iscritical for calculationsinvolving forward dynamics. The forward dynamicsrequiresthecalculationof themanipulatorinertiamatrixateachintegrationtimestep.TheToolboxusesa computationallysimplebut inefficient methodthat requiresevaluatingthe rne functionn 1 times,wheren is the numberof robot axes. For forward dynamicsthe rne is thebottleneck.

The Toolboxstoresall robot kinematicandinertial parametersin a robot object,but ac-cessingtheseparametersfrom a C languageMEX file is somewhatcumbersomeandmustbedoneon eachcall. Thereforethespeedadvantageincreaseswith thenumberof rows intheq, qd andqdd matricesthatareprovided. In otherwordsit is betterto call rne with atrajectory, thanfor eachpointon a trajectory.

To build theMEX file:

1. Changedirectoryto themex subdirectoryof theRoboticsToolbox.

1 INTRODUCTION 8

2. On a Unix systemjust typemake. For otherplatformsfollow theMathworksguide-lines. You needto compileandlink threefiles with a commandsomethinglike mexfrne.c ne.c vmath.c .

3. If successfulyounow haveafile calledfrne.ext whereext is thefile extensionanddependson thearchitecture(mexsol for Solaris,mexlx for Linux).

4. Fromwithin Matlabcd into this samedirectoryandrun thetestscript

>> 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


************************ ext force *****************************



***************************************************************

********************** Stanford arm ***************************

***************************************************************

************************ normal case *****************************



************************ no gravity *****************************



************************ ext force *****************************



>>

This will run theM-file andMEX-file versionsof the rne functionfor variousrobotmodelsandoptionswith variousoptions. For eachcaseit shouldreporta speedupgreaterthanone,andanerrorof zero.Theresultsshown abovearefor a SparcUltra10.

5. Copy the MEX-file frne.ext into the RoboticsToolbox main directory with thenamerne.ext . Thus all future referencesto rne will now invoke the MEX-fileinsteadof the M-file. The first time you run the MEX-file in any Matlab sessionitwill print aone-lineidentificationmessage.

9

2Using the Toolboxwith Simulink

2 Intr oduction

Simulink is the block diagramediting andsimulationenvironmentfor Matlab. Until itsmostrecentreleaseSimulinkhasnotbeenableto handlematrixvaluedsignals,andthathasmadeits applicationto roboticssomewhatclumsy. ThisshortcominghasbeenrectifiedwithSimulink Release4. RobotToolboxRelease7 andhigherincludesa library of blocksforusein constructingrobotkinematicanddynamicmodels.

To usethis new featureit is neccessaryto includetheToolboxSimulink block directoryinyourMatlabpath:

>> addpath ROBOTDIR/simulink

To bringup theblock library

>> roblocks

whichwill createa displaylike thatshown in Figure2.

Userswith no previous Simulink experienceareadvisedto readthe relevant Mathworksmanualsandexperimentwith the examplessupplied. ExperiencedSimulink usersshouldfind the useof the Roboticsblocksquite straightforward. Generallythereis a one-to-onecorrespondencebetweenSimulink blocksandToolbox functions. Several demonstrationshave beenincludedwith theToolbox in orderto illustratecommontopicsin robotcontrolanddemonstrateToolboxSimulink usage.Thesecouldbeconsideredasstartingpointsforyourown work, just selectthemodelclosestto whatyouwantandstartchangingit. Detailsof theblockscanbefoundusingtheFile/ShowBrowseroptionontheblock library window.

Robotics Toolbox for Matlab (release 7)

TODO

Copyright (c) 2002 Peter Corke

Dynamics Graphics Kinematics Transform conversionTrajectory

x

y

z

T

xyz2T

T1

T2

dx

tr2diff

roll

pitch

yaw

T

rpy2T

tau

q

qd

qdd

noname

rne

noname

plot

q

qd

qdd

jtraj

Jn

noname

q J

jacobn

J0

noname

q J

jacob0

J−1

J Ji

ijacob

q T

noname

fkine

a

b

c

T

eul2T

T

x

y

z

T2xyz

T

roll

pitch

yaw

T2rpy

T

a

b

c

T2eul

tau

q

qd

qdd

noname

Robot

Figure2: TheRoboticsToolboxblockset.

3 EXAMPLES 10

Puma560 collapsing under gravity

Puma 560

plot[0 0 0 0 0 0]’

Zerotorque

simout

To Workspace

tau

q

qd

qdd

Puma 560

Robot

Simple dynamics demopic11−Feb−2002 14:19:49

0

Clock

Figure3: RoboticsToolboxexampledemo1, Pumarobotcollapsingundergravity.

3 Examples

3.1 Dynamic simulation of Puma560robot collapsingunder gravity

TheSimulink model,demo1, is shown in Figure3, andthetwo blocksin this modelwouldbefamiliar to Toolboxusers.TheRobot block is similar to thefdyn() functionandrepre-sentstheforwarddynamicsof therobot,andtheplot block representstheplot function.Note the parametersof the Robot block containthe robot object to be simulatedandtheinitial joint angles.Theplot block hasoneparameterwhich is therobotobjectto bedis-playedgraphicallyandshouldbeconsistentwith therobotbeingsimulated.Displayoptionsaretakenfrom theplotbotopt.m file, seehelpfor robot/plot for details.

To runthisdemofirst createarobotobjectin theworkspace,typicallyby usingthepuma560

command,thenstartthesimulationusingSimulation/Startoptionfrom themodeltoolbar.

>> puma560

>> demo1

3.2 Dynamic simulation of a simple robot with flexible transmission

TheSimulinkmodel,demo2, is shown in Figure4,andrepresentsasimple2-link robotwithflexible or complianttransmission.Thefirst joint receivesasteppositiondemandchangeattime1s.Theresultingoscillationanddynamiccouplingbetweenthetwo jointscanbeseenclearly. Notethatthedrivemodelcomprisesspringplusdamper, andthatthejoint positioncontrolloopsaresimply unity negativefeedback.

To run this demofirst createa 2-link robot objectin the workspace,typicallyby usingthetwolink command,thenstartthesimulationusingSimulation/Startoptionfrom themodeltoolbar.

>> twolink

>> demo2

3 EXAMPLES 11

2−link robot with flexible transmission

load position

transmission comprisesspring + damper

assume the motoris infinitely "stiff"

Puma 560

plot

simout

To Workspace

Step

Scope

tau

q

qd

qdd

Puma 560

Robot

Rate Limiter

2−link demopicMon Apr 8 11:37:04 2002

100

K

du/dt

Derivative

0

Constant

0

Clock

20

B

motorposition

Figure4: RoboticsToolboxexampledemo2, simpleflexible 2-link manipulator.

3.3 Computed torquecontrol

TheSimulink model,demo3, shown in Figure5, is for a Puma560with a computedtorquecontrol structure.This is a “classical”dynamiccontrol techniquein which the rigid-bodydynamicmodelis invertedto computethedemandtorquefor therobotbasedoncurrentjointanglesandjoint angleratesanddemandjoint angleacceleration.Thismodelintroducestherne block which computesthe inversedynamicsusingthe recursive Newton-Euleralgo-rithm (seerne function),andthejtraj blockwhichcomputesavectorquinticpolynomial.jtraj hasparameterswhich includetheinitial andfinal valuesof theeachoutputelementaswell astheoverallmotiontime. Initial andfinal velocityareassumedto bezero.

In practiceof coursethe dynamicmodelof the robot is not exactly known, we canonlyinvert our bestestimateof the rigid-body dynamics. In the simulationwe canmodelthisby usingthe perturb function to alter the parametersof the dynamicmodelusedin therne block — notethe ’P/’ prefix on themodelnamedisplayedby thatblock. This meansthat theinversedynamicsarecomputedfor a slightly differentdynamicmodelto therobotundercontrolandshows theeffectof modelerroron controlperformance.



Puma 560 computed torque control

trajectory(demand)

robot state(actual)

error

tau

q

qd

qdd

P/Puma 560

rne

Puma 560

plot

q

qd

qdd

jtraj

simout

To Workspace

tau

q

qd

qdd

Puma 560

Robot

Puma560 computed torque controlpic11−Feb−2002 14:18:39

100

Kp

1

Kd

0

Clock

Figure5: RoboticsToolboxexampledemo3, computedtorquecontrol.

3 EXAMPLES 12

>> puma560

>> demo3

3.4 Torque feedforward control

The Simulink modeldemo4 demonstratestorquefeedforwardcontrol, another“classical”dynamiccontroltechniquein which thedemandedtorqueis computedusingtherne blockandaddedto theerrortorquecomputedfrom positionandvelocity error. It is instructive tocomparethestructureof this modelwith demo3. The inversedynamicsarenot in thefor-wardpathandsincetherobotconfigurationchangesrelatively slowly, they canbecomputedata low rate(this is illustratedby thezero-orderholdblock samplingat 20Hz).



>> puma560

>> demo4

3.5 Cartesianspacecontrol

TheSimulinkmodel,demo5, shown in Figure6, demonstratesCartesianspacemotioncon-trol. Thereare two conventionalapproachesto this. Firstly, resolve the Cartesianspacedemandto joint spaceusinginversekinematicsandthenperformthecontrolin joint space.Thesecond,usedhere,is to computetheerror in Cartesianspaceandresolve that to jointspacevia the inverseJacobian.This eliminatestheneedfor inversekinematicswithin thecontrolloop,andits attendentproblemsof multiple solutions.It alsoillustratessomeaddi-tionalSimulink blocks.

This demonstrationis for a Puma560 robot moving the tool in a circle of radius0.05mcenteredat the point 0 5 0 0 . The differencebetweenthe Cartesiandemandand thecurrentCartesianpose(in end-effector coordinates)is computedby the tr2diff blockwhich producesa differentialmotion describedby a 6-vector. The Jacobianblock hasasits input the currentmanipulatorjoint anglesandoutputsthe Jacobianmatrix. Sincethedifferentialmotion is with respectto the end-effector we usethe JacobianNblock ratherthanJacobian0.We usestandardSimulink block to invert the Jacobianandmultiply it by

Cartesian circle

Cartesian control

x

y

z

T

xyz2T

T1

T2

dx

tr2diff

Puma 560

plot

Bad Link

jacob0

J−1

J Ji

ijacob

q T

Puma 560

fkine

XY GraphT

x

y

z

T2xyz

1s

Ratecontrolled

robotaxes

MatrixMultiply

−0.6

0.05*sin(u)

f(u)

0

0

Clock

q

q

Figure6: RoboticsToolboxexampledemo5, Cartesianspacecontrol.

3 EXAMPLES 13

the differentialmotion. The result,after applicationof a simpleproportionalgain, is thejoint spacemotion requiredto correctthe Cartesianerror. The robot is modelledby anintegratorasasimpleratecontroldevice,or velocityservo.

Thisexamplealsodemonstratestheuseof the fkine block for forwardkinematicsandtheT2xyz block which extractsthetranslationalpartof therobot’s Cartesianstatefor plottingon theXY plane.

This demonstrationis very similar to thenumericalmethodusedto solve theinversekine-maticsproblemin ikine .



>> puma560

>> demo5

3.6 Image-basedvisual servoing

TheSimulinkmodel,demo6, shown in Figure7, demonstratesimage-basedvisualservoing(IBVS)[5]. This is quite a complex examplethat simulatesnot only the robot but alsoacameraandtheIBVS algorithm.Thecamerais assumedto bemountedon therobot’send-effectorandthis coordinateis passedinto thecamerablock so that therelative positionofthe target with respectto the cameracan be computed. Argumentsto the camerablockincludethe3D coordinatesof thetargetpoints. Theoutputof thecamerais the2D imageplanecoordinatesof the target points. The target points are usedto computean imageJacobianmatrix which is invertedandmultipliesthedesiredmotionof thetargetpointsonthe imageplane. Thedesiredmotion is simply thedifferencebetweenthe observedtargetpointsandthedesiredpoint positions.Theresultis a velocityscrew which drivestherobotto thedesiredposewith respectto a squaretarget.

Whenthesimulationstartsa new window, thecameraview, popsup. We seethat initiallythe squaretarget is off to one side and somewhat oblique. The imageplaneerrorsaremappedby animageJacobianinto desiredCartesianrates,andthesearefuthermappedby a

Image−based visual servo control

desired camera velocity

uv J

visualJacobian

146.1

visjaccondition

Puma 560

plot

MATLABFunction

pinv

6.998

manip jac condition

Jn

Puma 560

q J

jacobn

J−1

J Ji

ijacob

q T

Puma 560

fkine

274

feature errornorm

85.07

107.89

112.87

109.40

108.90

80.92

81.10

80.81

feature error

feature error

256 456456 456456 256256 256

desiredimage planecoordinates

MATLABFunction

cond()

T uv

camera

1s

Ratecontrolled

robotaxes

MatrixMultiply

MatrixMultiply

Image−based visual servo controlpic08−Apr−2002 11:31:20

MATLABFunction

MATLABFunction

MATLABFunction

−0.10

0.21

0.28

−0.32

−0.00

0.04

−0.01

Cartesian velocity dmd

6

q 6

66q

[6x6]

[6x6]

[6x6]

6

[4x2]

[4x2][4x2][4x4]

[8x6]

[8x6][8x6]

[4x2]

[6x8]

[4x2]

88

8

8

8feature vel

[6x6]

6

6

6

6

6

Figure7: RoboticsToolboxexampledemo6, image-basedvisualservoing.

3 EXAMPLES 14

manipulatorJacobianinto joint rateswhichareappliedto therobotwhich is againmodelledasa ratecontroldevice. Thisclosed-loopsystemis performingaCartesianpositioningtaskusinginformationfrom a cameraratherthanencodersanda kinematicmodel(theJacobianis a weakkinematicmodel). Image-basedvisualservoing schemeshave beenfound to beextremelyrobustwith respectto errorsin thecameramodelandmanipulatorJacobian.

15

3Tutorial

3 Manipulator kinematics

Kinematicsis thestudyof motionwithout regardto theforceswhich causeit. Within kine-maticsonestudiestheposition,velocityandacceleration,andall higherorderderivativesofthepositionvariables.Thekinematicsof manipulatorsinvolvesthestudyof thegeometricandtime basedpropertiesof themotion,andin particularhow thevariouslinks movewithrespectto oneanotherandwith time.

Typical robotsareserial-link manipulators comprisinga setof bodies,called links, in achain,connectedby joints1. Eachjoint hasonedegreeof freedom,eithertranslationalorrotational. For a manipulatorwith n joints numberedfrom 1 to n, therearen 1 links,numberedfrom 0 to n. Link 0 is the baseof the manipulator, generallyfixed, andlink ncarriestheend-effector. Joint i connectslinks i andi 1.

A link maybeconsideredasa rigid bodydefiningtherelationshipbetweentwo neighbour-ing joint axes.A link canbespecifiedby two numbers,thelink lengthandlink twist, whichdefinethe relative locationof the two axesin space.The link parametersfor the first andlast links aremeaningless,but arearbitrarily chosento be 0. Jointsmay be describedbytwo parameters.Thelink offsetis thedistancefrom onelink to thenext alongtheaxisof thejoint. The joint angleis therotationof onelink with respectto thenext aboutthejoint axis.

To facilitatedescribingthelocationof eachlink weaffix acoordinateframeto it — frameiis attachedto link i. Denavit andHartenberg[6] proposedamatrixmethodof systematicallyassigningcoordinatesystemsto eachlink of anarticulatedchain.Theaxisof revolutejointi is alignedwith zi 1. The xi 1 axis is directedalongthe normal from zi 1 to zi andforintersectingaxesis parallelto zi 1 zi . Thelink andjoint parametersmaybesummarizedas:

link length ai theoffsetdistancebetweenthezi 1 andzi axesalongthexi axis;

link twist αi theanglefrom thezi 1 axisto thezi axisaboutthexi axis;link offset di thedistancefrom theorigin of frame i 1 to the xi axis

alongthezi 1 axis;joint angle θi theanglebetweenthexi 1 andxi axesaboutthezi 1 axis.

For a revoluteaxisθi is the joint variableanddi is constant,while for a prismaticjoint di

is variable,andθi is constant.In many of the formulationsthat follow we usegeneralizedcoordinates,qi , where

qiθi for a revolutejointdi for a prismaticjoint

1Parallel link andserial/parallelhybridstructuresarepossible,thoughmuchlesscommonin industrialmanip-

ulators.

3 MANIPULATORKINEMATICS 16

joint i−1 joint i joint i+1

link i−1

link i

Ti−1

Tiai

Xi

YiZi

ai−1

Zi−1

Xi−1

Yi−1

(a)Standardformjoint i−1 joint i joint i+1

link i−1

link i

Ti−1 TiXi−1

Yi−1Zi−1

YiX

i

Zi

ai−1

ai

(b) Modified form

Figure8: Differentformsof Denavit-Hartenberg notation.

andgeneralizedforces

Qiτi for a revolutejointfi for aprismaticjoint

TheDenavit-Hartenberg (DH) representationresultsin a 4x4 homogeneoustransformationmatrix

i 1A i

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

0 sinαi cosαi di

0 0 0 1

(1)

representingeachlink’s coordinateframe with respectto the previous link’s coordinatesystem;thatis

0T i0T i 1

i 1A i (2)

where0T i is thehomogeneoustransformationdescribingtheposeof coordinateframei withrespectto theworld coordinatesystem0.

Two differing methodologieshave beenestablishedfor assigningcoordinateframes,eachof which allowssomefreedomin theactualcoordinateframeattachment:

1. Framei hasits origin alongtheaxisof joint i 1, asdescribedby Paul[1] andLee[2,7].

3 MANIPULATORKINEMATICS 17

2. Framei hasits origin alongtheaxisof joint i, andis frequentlyreferredto as‘modi-fied Denavit-Hartenberg’ (MDH) form[8]. This form is commonlyusedin literaturedealingwith manipulatordynamics.The link transformmatrix for this form differsfrom (1).

Figure8 shows thenotationaldifferencesbetweenthetwo forms.Notethatai is alwaysthelengthof link i, but is the displacementbetweenthe origins of frame i andframe i 1 inoneconvention,andframei 1 andframei in theother2. TheToolboxprovideskinematicfunctionsfor bothof theseconventions— thosefor modifiedDH parametersareprefixedby ‘m’.

3.1 Forward and inversekinematics

For an n-axis rigid-link manipulator, the forward kinematicsolutiongivesthe coordinateframe,or pose,of thelastlink. It is obtainedby repeatedapplicationof (2)

0Tn0A1

1A2n 1An (3)

K q (4)

which is the productof the coordinateframetransformmatricesfor eachlink. The poseof the end-effectorhas6 degreesof freedomin Cartesianspace,3 in translationand3 inrotation, so robot manipulatorscommonlyhave 6 joints or degreesof freedomto allowarbitraryend-effectorpose.Theoverallmanipulatortransform0Tn is frequentlywritten asTn, or T6 for a 6-axis robot. The forward kinematicsolutionmay be computedfor anymanipulator, irrespectiveof thenumberof jointsor kinematicstructure.

Of moreusein manipulatorpathplanningis the inversekinematicsolution

q K 1 T (5)

whichgivesthejoint anglesrequiredto reachthespecifiedend-effectorposition.In generalthis solution is non-unique,andfor someclassesof manipulatorno closed-formsolutionexists. If themanipulatorhasmorethan6 joints it is saidto be redundantandthesolutionfor joint anglesis under-determined.If no solutioncanbedeterminedfor a particularma-nipulatorposethat configurationis saidto be singular. The singularitymay be dueto analignmentof axesreducingthe effective degreesof freedom,or the point T beingout ofreach.

The manipulatorJacobianmatrix, Jθ, transformsvelocitiesin joint spaceto velocitiesofthe end-effector in Cartesianspace.For an n-axis manipulatorthe end-effectorCartesianvelocity is

0xn0Jθq (6)

tnxntnJθq (7)

in baseor end-effectorcoordinatesrespectively andwherex is theCartesianvelocity rep-resentedby a 6-vector. For a 6-axismanipulatorthe Jacobianis squareandprovided it isnot singularcanbeinvertedto solve for joint ratesin termsof end-effectorCartesianrates.TheJacobianwill not beinvertibleata kinematicsingularity, andin practicewill bepoorly

2Many paperswhentabulating the ‘modified’ kinematicparametersof manipulatorslist ai 1 andα i 1 not ai

andα i .

4 MANIPULATORRIGID-BODY DYNAMICS 18

conditionedin thevicinity of thesingularity, resultingin high joint rates.A controlschemebasedon Cartesianratecontrol

q 0J 1θ

0xn (8)

wasproposedby Whitney[9] andis known asresolvedratemotioncontrol. For two framesA andB relatedby ATB n o a p theCartesianvelocity in frameA maybetransformedtoframeB by

Bx BJAAx (9)

wheretheJacobianis givenby Paul[10] as

BJA f ATBn o a T p n p o p a T

0 n o a T (10)

4 Manipulator rigid-body dynamics

Manipulatordynamicsis concernedwith the equationsof motion, the way in which themanipulatormovesin responseto torquesappliedby theactuators,or externalforces.Thehistory and mathematicsof the dynamicsof serial-link manipulatorsis well coveredbyPaul[1] andHollerbach[11]. Therearetwo problemsrelatedto manipulatordynamicsthatareimportantto solve:

inversedynamicsin whichthemanipulator’sequationsof motionaresolvedfor givenmotionto determinethegeneralizedforces,discussedfurtherin Section4.1,and

direct dynamicsin which the equationsof motion are integratedto determinethegeneralizedcoordinateresponseto appliedgeneralizedforcesdiscussedfurther inSection4.2.

Theequationsof motionfor ann-axismanipulatoraregivenby

Q M q q C q q q F q G q (11)

where

q is thevectorof generalizedjoint coordinatesdescribingtheposeof themanipulatorq is thevectorof joint velocities;q is thevectorof joint accelerations

M is thesymmetricjoint-spaceinertiamatrix,or manipulatorinertiatensorC describesCoriolis and centripetaleffects — Centripetaltorquesare proportionalto q2

i ,while theCoriolis torquesareproportionalto qi qj

F describesviscousandCoulombfriction andis not generallyconsideredpart of the rigid-bodydynamics

G is thegravity loadingQ is thevectorof generalizedforcesassociatedwith thegeneralizedcoordinatesq.

The equationsmay be derived via a numberof techniques,including Lagrangian(energybased),Newton-Euler, d’Alembert[2, 12] or Kane’s[13] method.Theearliestreportedworkwasby Uicker[14] andKahn[15] usingtheLagrangianapproach.Dueto theenormouscom-putationalcost,O n4 , of this approachit wasnot possibleto computemanipulatortorquefor real-timecontrol. To achieve real-timeperformancemany approachesweresuggested,including tablelookup[16] andapproximation[17, 18]. The mostcommonapproximationwas to ignore the velocity-dependentterm C, sinceaccuratepositioningand high speedmotionareexclusivein typical robotapplications.


Method Multiplications Additions For N=6

Multiply Add

Lagrangian[22] 3212n4 86 5

12n3 25n4 6613n3 66,271 51,548

17114n2 531

3n 12912n2 421

3n

128 96

RecursiveNE[22] 150n 48 131n 48 852 738

Kane[13] 646 394

SimplifiedRNE[25] 224 174

Table1: Comparisonof computationalcostsfor inversedynamicsfrom varioussources.

Thelastentryis achievedby symbolicsimplificationusingthesoftwarepackageARM.

Orin etal.[19] proposedanalternativeapproachbasedontheNewton-Euler(NE) equationsof rigid-bodymotionappliedto eachlink. Armstrong[20] thenshowedhow recursionmightbeappliedresultingin O n complexity. Luh et al.[21] provideda recursive formulationofthe Newton-Eulerequationswith linear andangularvelocitiesreferredto link coordinateframes. They suggesteda time improvementfrom 7 9s for the Lagrangianformulationto 4 5ms, and thus it becamepractical to implement‘on-line’. Hollerbach[22] showedhow recursioncould be appliedto the Lagrangianform, andreducedthe computationtowithin a factorof 3 of therecursiveNE. Silver[23] showedtheequivalenceof therecursiveLagrangianand Newton-Euler forms, and that the differencein efficiency is due to therepresentationof angularvelocity.

“Kane’sequations”[13] provideanothermethodologyfor deriving theequationsof motionfor a specificmanipulator. A numberof ‘Z’ variablesare introduced,which while notnecessarilyof physicalsignificance,leadto adynamicsformulationwith low computationalburden.Wampler[24] discussesthecomputationalcostsof Kane’smethodin somedetail.

The NE andLagrangeforms canbe written generallyin termsof the Denavit-Hartenbergparameters— however the specificformulations,suchasKane’s, canhave lower compu-tationalcostfor the specificmanipulator. Whilst the recursive forms arecomputationallymoreefficient, the non-recursive forms computethe individual dynamicterms(M , C andG) directly. A comparisonof computationcostsis givenin Table1.

4.1 RecursiveNewton-Euler formulation

Therecursive Newton-Euler(RNE) formulation[21] computestheinversemanipulatordy-namics,that is, the joint torquesrequiredfor a given set of joint angles,velocitiesandaccelerations.The forward recursionpropagateskinematicinformation— suchasangu-lar velocities,angularaccelerations,linearaccelerations— from thebasereferenceframe(inertial frame)to theend-effector. Thebackwardrecursionpropagatestheforcesandmo-mentsexertedon eachlink from the end-effectorof the manipulatorto the basereferenceframe3. Figure9 shows thevariablesinvolvedin thecomputationfor onelink.

The notationof Hollerbach[22] andWalker andOrin [26] will be usedin which the leftsuperscriptindicatesthereferencecoordinateframefor thevariable.Thenotationof Luh etal.[21] andlaterLee[7, 2] is considerablylessclear.

3It shouldbenotedthatusingMDH notationwith its differentaxisassignmentconventionstheNewton Euler

formulationis expresseddifferently[8].


joint i−1 joint i joint i+1

link i−1

link i

Ti−1

Tiai

Xi

YiZi

ai−1

Zi−1

Xi−1

Yi−1p* vi

.vi

.ωiωi

n fi i

N Fi i

vi

.vi

_ _i+1 i+1

n f

si

Figure9: Notationusedfor inversedynamics,basedon standardDenavit-Hartenberg nota-

tion.

Outward recursion,1 i n.

If axis i 1 is rotationali 1ωi 1

i 1Riiωi z0q

i 1(12)

i 1ωi 1i 1Ri

iωi z0qi 1

iωi z0qi 1

(13)

i 1vi 1i 1ωi 1

i 1pi 1

i 1Riivi (14)

i 1vi 1i 1ωi 1

i 1pi 1

i 1ωi 1i 1ωi 1

i 1pi 1

i 1Rii vi (15)

If axis i 1 is translationali 1ωi 1

i 1Riiωi (16)

i 1ωi 1i 1Ri

iωi (17)

i 1vi 1i 1Ri z0q

i 1ivi

i 1ωi 1i 1p

i 1(18)

i 1vi 1i 1Ri z0q

i 1i vi

i 1ωi 1i 1p

i 12 i 1ωi 1

i 1Ri z0qi 1

i 1ωi 1i 1ωi 1

i 1pi 1

(19)

i viiωi si

iωiiωi si

i vi (20)iF i mi

i vi (21)iNi Ji

iωiiωi Ji

iωi (22)

Inward recursion,n i 1.

i fi

iRi 1i 1 f

i 1iFi (23)

iniiRi 1

i 1ni 1i 1Ri

i pi

i i 1 fi 1

i pi

siiF i

iNi (24)

Qi

iniT iRi 1z0 if link i 1 is rotational

i fi

TiRi 1z0 if link i 1 is translational

(25)

where


i is thelink index, in therange1 to nJi is themomentof inertiaof link i aboutits COMsi is thepositionvectorof theCOM of link i with respectto framei

ωi is theangularvelocityof link iωi is theangularaccelerationof link ivi is thelinearvelocityof frameivi is thelinearaccelerationof frameivi is thelinearvelocityof theCOM of link ivi is thelinearaccelerationof theCOM of link ini is themomentexertedon link i by link i 1f

iis theforceexertedon link i by link i 1

Ni is thetotal momentat theCOM of link iF i is thetotal forceat theCOM of link iQ

iis theforceor torqueexertedby theactuatorat joint i

i 1Ri is the orthonormalrotationmatrix definingframe i orientationwith respectto framei 1. It is theupper3 3 portionof thelink transformmatrixgivenin (1).

i 1Ri

cosθi cosαi sinθi sinαi sinθi

sinθi cosαi cosθi sinαi cosθi

0 sinαi cosαi

(26)

iRi 1i 1Ri

1 i 1RiT (27)

i pi

is thedisplacementfrom theorigin of framei 1 to framei with respectto framei.

i pi

ai

di sinαi

di cosαi

(28)

It is thenegativetranslationalpartof i 1A i1.

z0 is a unit vectorin Z direction,z0 0 0 1

NotethattheCOM linearvelocity givenby equation(14) or (18) doesnot needto becom-putedsincenootherexpressiondependsuponit. Boundaryconditionsareusedto introducetheeffectof gravity by settingtheaccelerationof thebaselink

v0 g (29)

whereg is the gravity vector in the referencecoordinateframe, generallyacting in thenegativeZ direction,downward.Basevelocity is generallyzero

v0 0 (30)

ω0 0 (31)

ω0 0 (32)

At this stagetheToolboxonly providesanimplementationof thisalgorithmusingthestan-dardDenavit-Hartenberg conventions.

4.2 Dir ectdynamics

Equation(11) may be usedto computethe so-calledinversedynamics,that is, actuatortorqueasa functionof manipulatorstateandis usefulfor on-line control. For simulation

REFERENCES 22

thedirect,integral or forward dynamicformulationis requiredgiving joint motionin termsof input torques.

Walker andOrin[26] describeseveral methodsfor computingthe forward dynamics,andall make useof an existing inversedynamicssolution. Using the RNE algorithmfor in-versedynamics,thecomputationalcomplexity of the forwarddynamicsusing‘Method 1’is O n3 for ann-axismanipulator. Theirothermethodsareincreasinglymoresophisticatedbut reducethe computationalcost,thoughstill O n3 . Featherstone[27] hasdescribedthe“articulated-bodymethod”for O n computationof forwarddynamics,however for n 9it is more expensive than the approachof Walker andOrin. AnotherO n approachforforwarddynamicshasbeendescribedby Lathrop[28].

4.3 Rigid-body inertial parameters

Accuratemodel-baseddynamiccontrol of a manipulatorrequiresknowledgeof the rigid-bodyinertial parameters.Eachlink hastenindependentinertial parameters:

link mass,mi ;

threefirst moments,whichmaybeexpressedastheCOM location,si , with respecttosomedatumon thelink or asa momentSi misi ;

six secondmoments,which representthe inertiaof thelink abouta givenaxis,typi-cally throughtheCOM. Thesecondmomentsmaybeexpressedin matrix or tensorform as

JJxx Jxy Jxz

Jxy Jyy Jyz

Jxz Jyz Jzz

(33)

wherethe diagonalelementsare the momentsof inertia, and the off-diagonalsareproductsof inertia. Only six of thesenineelementsareunique: threemomentsandthreeproductsof inertia.

For any point in a rigid-bodythereis onesetof axesknown astheprincipal axesofinertia for which theoff-diagonalterms,or products,arezero. Theseaxesaregivenby the eigenvectorsof the inertia matrix (33) andthe eigenvaluesare the principalmomentsof inertia. Frequentlytheproductsof inertiaof therobotlinks arezerodueto symmetry.

A 6-axismanipulatorrigid-bodydynamicmodelthusentails60 inertial parameters.Theremaybeadditionalparametersper joint dueto friction andmotorarmatureinertia. Clearly,establishingnumericvaluesfor this numberof parametersis adifficult task.Many parame-terscannotbemeasuredwithoutdismantlingtherobotandperformingcarefulexperiments,thoughthis approachwasusedby Armstronget al.[29]. Most parameterscouldbederivedfrom CAD modelsof the robots,but this informationis often consideredproprietaryandnotmadeavailableto researchers.

References

[1] R. P. Paul, RobotManipulators: Mathematics,Programming, and Control. Cam-bridge,Massachusetts:MIT Press,1981.

REFERENCES 23

[2] K. S. Fu, R. C. Gonzalez,andC. S. G. Lee, Robotics.Control, Sensing, Vision andIntelligence. McGraw-Hill, 1987.

[3] M. SpongandM. Vidyasagar, RobotDynamicsand Control. JohnWiley andSons,1989.

[4] J.J.Craig,Introductionto Robotics. AddisonWesley, 1986.

[5] S. Hutchinson,G. Hager, and P. Corke, “A tutorial on visual servo control,” IEEETransactionson RoboticsandAutomation, vol. 12,pp.651–670,Oct.1996.

[6] R. S. Hartenberg andJ. Denavit, “A kinematicnotationfor lower pair mechanismsbasedon matrices,” Journalof AppliedMechanics, vol. 77,pp.215–221,June1955.

[7] C. S.G.Lee,“Robotarmkinematics,dynamicsandcontrol,” IEEEComputer, vol. 15,pp.62–80,Dec.1982.

[8] J.J.Craig,Introductionto Robotics. AddisonWesley, seconded.,1989.

[9] D. Whitney, “The mathematicsof coordinatedcontrolof prostheticarmsandmanipu-lators,” ASMEJournalof DynamicSystems,MeasurementandControl, vol. 20,no.4,pp.303–309,1972.

[10] R. P. Paul, B. Shimano,andG. E. Mayer, “Kinematic control equationsfor simplemanipulators,” IEEE Trans.Syst.ManCybern., vol. 11,pp.449–455,June1981.

[11] J. M. Hollerbach,“Dynamics,” in RobotMotion - Planningand Control (M. Brady,J.M. Hollerbach,T. L. Johnson,T. Lozano-Perez,andM. T. Mason,eds.),pp.51–71,MIT, 1982.

[12] C. S. G. Lee,B. Lee,andR. Nigham,“Developmentof the generalizedD’Alembertequationsof motionfor mechanicalmanipulators,” in Proc.22ndCDC, (SanAntonio,Texas),pp.1205–1210,1983.

[13] T. KaneandD. Levinson,“The useof Kane’sdynamicalequationsin robotics,” Int. J.Robot.Res., vol. 2, pp.3–21,Fall 1983.

[14] J. Uicker, On theDynamicAnalysisof SpatialLinkagesUsing4 by 4 Matrices. PhDthesis,Dept.MechanicalEngineeringandAstronauticalSciences,NorthWesternUni-versity, 1965.

[15] M. Kahn, “The near-minimum time control of open-looparticulatedkinematiclink-ages,” Tech.Rep.AIM-106, StanfordUniversity, 1969.

[16] M. H. RaibertandB. K. P. Horn, “Manipulatorcontrolusingtheconfigurationspacemethod,” TheIndustrial Robot, pp.69–73,June1978.

[17] A. Bejczy, “Robot armdynamicsandcontrol,” Tech.Rep.NASA-CR-136935,NASAJPL,Feb. 1974.

[18] R. Paul, “Modelling, trajectory calculationand servoing of a computercontrolledarm,” Tech.Rep. AIM-177, StanfordUniversity, Artificial IntelligenceLaboratory,1972.

[19] D. Orin, R. McGhee,M. Vukobratovic, and G. Hartoch, “Kinematics and kineticanalysisof open-chainlinkagesutilizing Newton-Eulermethods,” MathematicalBio-sciences.An InternationalJournal, vol. 43,pp.107–130,Feb. 1979.

REFERENCES 24

[20] W. Armstrong,“Recursive solutionto theequationsof motionof ann-link manipula-tor,” in Proc.5thWorld CongressonTheoryof MachinesandMechanisms, (Montreal),pp.1343–1346,July1979.

[21] J.Y. S.Luh, M. W. Walker, andR. P. C. Paul,“On-line computationalschemefor me-chanicalmanipulators,” ASMEJournal of DynamicSystems,MeasurementandCon-trol, vol. 102,pp.69–76,1980.

[22] J. Hollerbach,“A recursive Lagrangianformulationof manipulatordynamicsandacomparative studyof dynamicsformulationcomplexity,” IEEE Trans.Syst.Man Cy-bern., vol. SMC-10,pp.730–736,Nov. 1980.

[23] W. M. Silver, “On the equivalanceof Lagrangianand Newton-Eulerdynamicsformanipulators,” Int. J. Robot.Res., vol. 1, pp.60–70,Summer1982.

[24] C. Wampler, ComputerMethodsin ManipulatorKinematics,Dynamics,andControl:a ComparativeStudy. PhDthesis,StanfordUniversity, 1985.

[25] J. J. Murray, ComputationalRobotDynamics. PhD thesis,Carnegie-MellonUniver-sity, 1984.

[26] M. W. Walker and D. E. Orin, “Efficient dynamiccomputersimulationof roboticmechanisms,” ASME Journal of Dynamic Systems,Measurement and Control,vol. 104,pp.205–211,1982.

[27] R. Featherstone,RobotDynamicsAlgorithms. Kluwer AcademicPublishers,1987.

[28] R. Lathrop,“Constrained(closed-loop)robotsimulationby local constraintpropoga-tion.,” in Proc. IEEE Int. Conf. RoboticsandAutomation, pp.689–694,1986.

[29] B. Armstrong,O. Khatib, andJ. Burdick, “The explicit dynamicmodelandinertialparametersof thePuma560arm,” in Proc. IEEE Int. Conf. RoboticsandAutomation,vol. 1, (Washington,USA), pp.510–18,1986.

1

2Reference

For ann-axismanipulatorthefollowing matrixnaminganddimensionalconventionsapply.

Symbol Dimensions Descriptionl link manipulatorlink objectq 1 n joint coordinatevectorq m n m-point joint coordinatetrajectoryqd 1 n joint velocityvectorqd m n m-point joint velocity trajectoryqdd 1 n joint accelerationvectorqdd m n m-point joint accelerationtrajectoryrobot robot robotobjectT 4 4 homogeneoustransformT 4 4 m m-pointhomogeneoustransformtrajectoryQ quaternion unit-quaternionobjectM 1 6 vectorwith elementsof 0 or 1 correspondingto

CartesianDOF alongX, Y, Z andaroundX, Y, Z.1 if thatCartesianDOF belongsto thetaskspace,else0.

v 3 1 Cartesianvectort m 1 timevectord 6 1 differentialmotionvector

Objectnamesareshown in bold typeface.

A trajectoryis representedby a matrix in which eachrow correspondsto oneof m timesteps. For a joint coordinate,velocity or accelerationtrajectorythe columnscorrespondto the robotaxes. For homogeneoustransformtrajectorieswe use3-dimensionalmatriceswherethelastindex correspondsto thetimestep.

Units

All anglesarein radians.Thechoiceof all otherunitsis up to theuser, andthischoicewillflow on to theunits in which homogeneoustransforms,Jacobians,inertiasandtorquesarerepresented.

RoboticsToolboxRelease7 PeterCorke,April 2002

Introduction 2

Homog eneous Transf ormseul2tr Eulerangleto homogeneoustransformoa2tr orientationandapproachvectorto homogeneoustransformrotvec homogeneoustransformfor rotationaboutarbitraryvectorrotx homogeneoustransformfor rotationaboutX-axisroty homogeneoustransformfor rotationaboutY-axisrotz homogeneoustransformfor rotationaboutZ-axisrpy2tr Roll/pitch/yaw anglesto homogeneoustransformtr2eul homogeneoustransformto Euleranglestr2rot homogeneoustransformto rotationsubmatrixtr2rpy homogeneoustransformto roll/pitch/yaw anglestransl set or extract the translationalcomponentof a homoge-

neoustransformtrnorm normalizeahomogeneoustransform

Trajector y Generationctraj Cartesiantrajectoryjtraj joint spacetrajectorytrinterp interpolatehomogeneoustransforms

Quaternions/ dividequaternionby quaternionor scalar* multiply quaternionby aquaternionor vectorinv inverta quaternionnorm normof a quaternionplot displayaquaternionasa 3D rotationq2tr quaternionto homogeneoustransformquaternion constructa quaternionqinterp interpolatequaternionsunit unitizeaquaternion

Manipulator Modelslink constructa robotlink objectnofriction removefriction from arobotobjectperturb randomlymodify somedynamicparameterspuma560 Puma560datapuma560akb Puma560data(modifiedDenavit-Hartenberg)robot constructa robotobjectshowlink show link/robotdatain detailstanford Stanfordarmdatatwolink simple2-link example


Introduction 3

Kinematicsdiff2tr differentialmotionvectorto transformfkine computeforwardkinematicsftrans transformforce/momentikine computeinversekinematicsikine560 computeinversekinematicsfor Puma560likearmjacob0 computeJacobianin basecoordinateframejacobn computeJacobianin end-effectorcoordinateframetr2diff homogeneoustransformto differentialmotionvectortr2jac homogeneoustransformto Jacobian

Graphicsdrivebot drivea graphicalrobotplot plot/animaterobot

Dynamicsaccel computeforwarddynamicscinertia computeCartesianmanipulatorinertiamatrixcoriolis computecentripetal/coriolistorquefdyn forwarddynamics(motiongivenforces)friction joint frictiongravload computegravity loadinginertia computemanipulatorinertiamatrixitorque computeinertiatorquerne inversedynamics(forcesgivenmotion)

Otherishomog testif matrix is 4 4maniplty computemanipulabilityrtdemo toolboxdemonstrationunit unitizeavector


accel 4

accel

Purpose Computemanipulatorforwarddynamics

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

Description Returnsa vectorof joint accelerationsthatresultfrom applyingtheactuatortorque to themanipulatorrobot with joint coordinatesq andvelocitiesqd.

Algorithm Usesthe method1 of Walker andOrin to computethe forward dynamics. This form isusefulfor simulationof manipulatordynamics,in conjunctionwith a numericalintegrationfunction.

See Also rne,robot,fdyn, ode45

References M. W. Walker andD. E. Orin. Efficient dynamiccomputersimulationof robotic mecha-nisms.ASMEJournalof DynamicSystems,MeasurementandControl, 104:205–211,1982.


cinertia 5

cinertia

Purpose ComputetheCartesian(operationalspace)manipulatorinertiamatrix

Synopsis M = cinertia(robot, q)

Description cinertia computesthe Cartesian,or operationalspace,inertia matrix. robot is a robotobjectthatdescribesthemanipulatordynamicsandkinematics,andq is ann-elementvectorof joint coordinates.

Algorithm TheCartesianinertiamatrix is calculatedfrom thejoint-spaceinertiamatrixby

M x J q TM q J q 1

andrelatesCartesianforce/torqueto Cartesianacceleration

F M x x

See Also inertia,robot,rne

References O. Khatib, “A unified approachfor motion and force control of robot manipulators:theoperationalspaceformulation,” IEEETrans.Robot.Autom., vol. 3, pp.43–53,Feb. 1987.


coriolis 6

coriolis

Purpose ComputethemanipulatorCoriolis/centripetaltorquecomponents

Synopsis tau c = coriolis(robot, q, qd)

Description coriolis returnsthejoint torquesdueto rigid-bodyCoriolisandcentripetaleffectsfor thespecifiedjoint stateq andvelocityqd. robot isarobotobjectthatdescribesthemanipulatordynamicsandkinematics.

If q andqd arerow vectors,tau c is a row vectorof joint torques.If q andqd arematrices,eachrow is interpretedasa joint statevector, and tau c is a matrix eachrow being thecorrespondingjoint torques.

Algorithm Evaluatedfrom theequationsof motion,usingrne , with joint accelerationandgravitationalaccelerationsetto zero,

τ C q q q

Jointfriction is ignoredin this calculation.

See Also robot,rne,itorque,gravload

References M. W. Walker andD. E. Orin. Efficient dynamiccomputersimulationof robotic mecha-nisms.ASMEJournalof DynamicSystems,MeasurementandControl, 104:205–211,1982.


ctraj 7

ctraj

Purpose ComputeaCartesiantrajectorybetweentwo points

Synopsis TC = ctraj(T0, T1, m)

TC = ctraj(T0, T1, r)

Description ctraj returnsaCartesiantrajectory(straightline motion)TC from thepoint representedbyhomogeneoustransformT0 to T1. Thenumberof pointsalongthepathis mor thelengthofthegivenvectorr . For thesecondcaser is avectorof distancesalongthepath(in therange0 to 1) for eachpoint. Thefirst casehasthepointsequallyspaced,but differentspacingmaybespecifiedto achieveacceptableaccelerationprofile. TC is a4 4 mmatrix.

Examples To createaCartesianpathwith smoothaccelerationwecanusethejtraj functionto createthepathvectorr with continuousderivitives.

>> 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));

0 1 2 3 4 5 6 7 8 9 10−1

−0.5

0

0.5

1

1.5

2

Time (s)

See Also trinterp,qinterp,transl

References R. P. Paul, RobotManipulators: Mathematics,Programming, and Control. Cambridge,Massachusetts:MIT Press,1981.


diff2tr 8

diff2tr

Purpose Converta differentialmotionvectorto ahomogeneoustransform

Synopsis delta = diff2tr(x)

Description Returnsa homogeneoustransformrepresentingdifferentialtranslationandrotationcorre-spondingto Cartesianvelocityx vx vy vz ωx ωy ωz .

Algorithm Frommechanicswe know thatR Sk Ω R

whereR is anorthonormalrotationmatrix and

Sk Ω0 ωz ωy

ωz 0 ωx

ωy ωx 0

andis a skew-symmetricmatrix. Thiscanbegeneralizedto

TSk Ω P000 1

T

for therotationalandtranslationalcase.

See Also tr2diff

References R. P. Paul. RobotManipulators: Mathematics,Programming, and Control. MIT Press,Cambridge,Massachusetts,1981.


drivebot 9

dri vebot

Purpose Drivea graphicalrobot

Synopsis drivebot(robot)

drivebot(robot, q)

Description Popsup a window with oneslider for eachjoint. Operationof the sliderswill drive thegraphicalroboton thescreen.Very usefulfor gaininganunderstandingof joint limits androbotworkspace.

The joint coordinatestateis kept with the graphicalrobot andcanbe obtainedusing theplot function. If q is specifiedit is usedastheinitial joint angle,otherwisetheinitial valueof joint coordinatesis takenfrom thegraphicalrobot.

Examples To drivea graphicalPuma560robot

>> puma560 % define the robot

>> plot(p560,qz) % draw it

>> drivebot(p560) % now drive it

See Also robot/plot,robot


eul2tr 10

eul2tr

Purpose ConvertEuleranglesto a homogeneoustransform

Synopsis T = eul2tr([r p y])

T = eul2tr(r,p,y)

Description eul2tr returnsahomogeneoustransformationfor thespecifiedEuleranglesin radians.

T RZ a RY b RZ c

Cautionar y Notethat12differentEuleranglesetsor conventionsexist. Theconventionusedhereis thecommononefor robotics,but is not theoneusedfor examplein theaerospacecommunity.

See Also tr2eul,rpy2tr

References R. P. Paul, RobotManipulators: Mathematics,Programming, and Control. Cambridge,Massachusetts:MIT Press,1981.


fdyn 11

fdyn

Purpose Integrateforwarddynamics

Synopsis [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 integratesthe manipulatorequationsof motion over the time interval t0 to t1 us-ing MATLAB ’s ode45 numericalintegrationfunction.Manipulatorkinematicanddynamicchacteristicsaregivenby therobotobjectrobot . It returnsa timevectort , andmatricesofmanipulatorjoint stateq andjoint velocitiesqd. Thesematriceshaveonerow pertimestepandonecolumnperjoint.

Actuatortorquemaybespecifiedby a userfunction

tau = torqfun(t, q, qd, arg1, arg2, ...)

wheret is thecurrenttime,andq andqd arethemanipulatorjoint coordinateandvelocitystaterespectively. Optionalargumentspassedto fdyn will be passedthroughto the userfunction. Typically this function would be usedto implementsomeaxis control schemeasa function of manipulatorstateandpassedin setpointinformation. If torqfun is notspecifiedthenzerotorqueis appliedto themanipulator.

Initial joint coordinatesandvelocitiesmaybe specifiedby the optionalargumentsq0 andqd0 respectively.

Algorithm Thejoint accelerationis a functionof joint coordinateandvelocitygivenby

q M q 1 τ C q q q G q F q

Example Thefollowingexampleshowshow fdyn() canbeusedtosimulatearobotanditscontroller.The manipulatoris a Puma560 with simple proportionaland derivative controller. Thesimulationresultsareshown in thefigure,andfurthergaintuningis clearlyrequired.Notethathigh gainsarerequiredon joints 2 and3 in orderto counterthesignificantdisturbancetorquedueto 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);


fdyn 12

Notetheuseof qz a zerovectorof length6 definedby puma560 padsout the two initial condition

arguments,andwe placethecontrolgainsandthepathasoptionalarguments.Notealsotheuseof

thenofriction() function,seeCautionarynotebelow. Theinvokedfunctionis

%

% 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 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−2

−1

0

1

Time (s)

Join

t 3 (

rad)

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−1

0

1

2

Time (s)

Join

t 2 (

rad)

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5−0.05

0

0.05

Time (s)

Join

t 1 (

rad)

Resultsof fdyn() example.Simulatedpathshown assolid,andreferencepathasdashed.

Cautionar y Thepresenceof non-linearfriction in thedynamicmodelcanpreventtheintegrationfrom converging.

Thefunctionnofriction() canbeusedto returna Coulombfriction freerobotobject.


fdyn 13

See Also accel,nofriction,rne,robot,ode45

References M. W. Walker andD. E. Orin. Efficientdynamiccomputersimulationof roboticmechanisms.ASME

Journalof DynamicSystems,MeasurementandControl, 104:205–211,1982.


fkine 14

fkine

Purpose Forwardrobotkinematicsfor seriallink manipulator

Synopsis T = fkine(robot, q)

Description fkine computesforwardkinematicsfor thejoint coordinateq giving a homogeneoustransformfor

thelocationof theend-effector. robot is a robotobjectwhich containsa kinematicmodelin either

standardor modifiedDenavit-Hartenberg notation.Notethattherobotobjectcanspecifyanarbitrary

homogeneoustransformfor thebaseof therobotanda tool offset.

If q is avectorit is interpretedasthegeneralizedjoint coordinates,andfkine returnsahomogeneous

transformationfor thefinal link of themanipulator. If q is a matrix eachrow is interpretedasa joint

statevector, andT is a 4 4 m matrixwheremis thenumberof rows in q.

Cautionar y Notethatthedimensionalunitsfor thelastcolumnof theT matrixwill bethesameasthedimensional

units usedin the robot object. The units canbe whatever you choose(metres,inches,cubitsor

furlongs)but thechoicewill affect thenumericalvalueof theelementsin the lastcolumnof T. The

Toolboxdefinitionspuma560 andstanford all useSI unitswith dimensionsin metres.

See Also link, robot

References R. P. Paul. RobotManipulators: Mathematics,Programming, andControl. MIT Press,Cambridge,

Massachusetts,1981.

J.J.Craig,Introductionto Robotics. AddisonWesley, seconded.,1989.


link/friction 15

link/friction

Purpose Computejoint friction torque

Synopsis tau f = friction(link, qd)

Description friction computesthe joint friction torquebasedon friction parameterdata,if any, in the link

objectlink . Friction is a functiononly of joint velocityqd

If qd is a vector then tau f is a vector in which eachelementis the friction torquefor the the

correspondingelementin qd .

Algorithm The friction model is a fairly standardone comprisingviscousfriction and direction dependent

Coulombfriction

Fi tBi q τi θ 0

Bi q τi θ 0

See Also link,robot/friction,nofriction


robot/friction 16

robot/friction

Purpose Computerobotfriction torquevector

Synopsis tau f = friction(robot, qd)

Description friction computesthejoint friction torquevectorfor therobotobjectrobot with a joint velocity

vectorqd .

See Also link, link/friction, nofriction


ftrans 17

ftrans

Purpose Forcetransformation

Synopsis F2 = ftrans(F, T)

Description TransformtheforcevectorF in thecurrentcoordinateframeto forcevectorF2 in thesecondcoordi-

nateframe. Thesecondframeis relatedto thefirst by thehomogeneoustransformT. F2 andF are

each6-elementvectorscomprisingforceandmomentcomponentsFx Fy Fz Mx My Mz .

See Also diff2tr


gravload 18

gravload

Purpose Computethemanipulatorgravity torquecomponents

Synopsis tau g = gravload(robot, q)

tau g = gravload(robot, q, grav)

Description gravload computesthejoint torquedueto gravity for themanipulatorin poseq.

If q is arow vector, tau g returnsarow vectorof joint torques.If q is amatrixeachrow is interpreted

asasa joint statevector, and tau g is a matrix in which eachrow is thegravity torquefor the the

correspondingrow in q.

Thedefaultgravity directioncomesfrom therobotobjectbut maybeoverriddenby theoptionalgrav

argument.

See Also robot,link, rne,itorque,coriolis




ikine 19

ikine

Purpose Inversemanipulatorkinematics

Synopsis q = ikine(robot, T)

q = ikine(robot, T, q0)

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

Description ikine returnsthejoint coordinatesfor themanipulatordescribedby theobjectrobot whoseend-

effectorhomogeneoustransformis givenby T. Notethattherobot’s basecanbearbitrarily specified

within therobotobject.

If T is a homogeneoustransformthena row vectorof joint coordinatesis returned.Theestimatefor

thefirst stepis q0 if this is givenelse0.

If T is a homogeneoustransformtrajectoryof size4 4 m thenq will be an n m matrix where

eachrow is a vectorof joint coordinatescorrespondingto thelastsubscriptof T. Theestimatefor the

first stepin thesequenceis q0 if this is given else0. The initial estimateof q for eachtime stepis

takenasthesolutionfrom theprevioustimestep.

Notethat the inversekinematicsolutionis generallynot unique,anddependson the initial valueq0

(whichdefaultsto 0).

For thecaseof a manipulatorwith fewer than6 DOF it is not possiblefor theend-effector to satisfy

theend-effectorposespecifiedby anarbitraryhomogeneoustransform.This typically leadsto non-

convergencein ikine . A solution is to specifya 6-elementweightingvector, M, whoseelements

are 0 for thoseCartesianDOF that are unconstrainedand 1 otherwise. The elementscorrespond

to translationalong the X-, Y- and Z-axes and rotation about the X-, Y- and Z-axes respectively.

For example,a 5-axismanipulatormaybe incapableof independantlycontrolling rotationaboutthe

end-effector’sZ-axis. In thiscaseM = [1 1 1 1 1 0] wouldenableasolutionin whichtheend-

effector adoptedthe poseT exceptfor the end-effector rotation. The numberof non-zeroelements

shouldequalthenumberof robotDOF.

Algorithm Thesolutionis computediteratively usingthepseudo-inverseof themanipulatorJacobian.

q J q ∆ F q T

where∆ returnsthe‘dif ference’betweentwo transformsasa 6-elementvectorof displacementsand

rotations(seetr2diff ).

Cautionar y Sucha solution is completelygeneral,thoughmuch lessefficient than specific inversekinematic

solutionsderivedsymbolically.

Thereturnedjoint anglesmaybein non-minimumform, ie. q 2nπ.

This approachallows a solutionto obtainedat a singularity, but the joint coordinateswithin thenull

spacearearbitrarily assigned.


ikine 20

Notethat thedimensionalunitsusedfor the lastcolumnof theT matrix mustagreewith thedimen-

sionalunits usedin the robot definition. Theseunits canbe whatever you choose(metres,inches,

cubitsor furlongs)but they mustbeconsistent.TheToolboxdefinitionspuma560 andstanford

all useSI unitswith dimensionsin metres.

See Also fkine, tr2diff, jacob0,ikine560,robot

References S. Chieaverini, L. Sciavicco, andB. Siciliano,“Control of roboticsystemsthroughsingularities,” in

Proc. Int. Workshopon Nonlinearand AdaptiveControl: Issuesin Robotics(C. C. de Wit, ed.),

Springer-Verlag,1991.


ikine560 21

ikine560

Purpose Inversemanipulatorkinematicsfor Puma560likearm

Synopsis q = ikine560(robot, config)

Description ikine560 returnsthe joint coordinatescorrespondingto the end-effector homogeneoustransform

T. It is computedusinga symbolicsolutionappropriatefor Puma560like robots,that is, all revolute

6DOF arms,with a sphericalwrist. The useof a symbolicsolutionmeansthat it executesover 50

timesfasterthanikine for a Puma560solution.

A furtheradvantageis thatikine560() allowscontrolover thespecificsolutionreturned.config

is a stringwhichcontainsoneor moreof theconfigurationcontrollettercodes

’l’ left-handed(lefty) solution(default)

’r’ †right-handed(righty) solution

’u’ †elbow upsolution(default)

’d’ elbow down solution

’f’ †wrist flippedsolution

’n’ wrist not flippedsolution(default)

Cautionar y Notethat thedimensionalunitsusedfor the lastcolumnof theT matrix mustagreewith thedimen-

sionalunitsusedin therobot object.Theseunitscanbewhateveryouchoose(metres,inches,cubits

or furlongs)but they mustbeconsistent.TheToolboxdefinitionspuma560 andstanford all use

SI unitswith dimensionsin metres.

See Also fkine, ikine, robot

References R. P. Paul and H. Zhang, “Computationallyefficient kinematicsfor manipulatorswith spherical

wrists,” Int. J. Robot.Res., vol. 5, no.2, pp.32–44,1986.

Author RobertBiro andGaryMcMurray, Georgia Instituteof Technology, [email protected]


inertia 22

inertia

Purpose Computethemanipulatorjoint-spaceinertiamatrix

Synopsis M = inertia(robot, q)

Description inertia computesthejoint-spaceinertiamatrix which relatesjoint torqueto joint acceleration

τ M q q

robot is a robot object that describesthe manipulatordynamicsand kinematics,and q is an n-

elementvectorof joint state.For ann-axismanipulatorMis ann n symmetricmatrix.

If q is a matrix eachrow is interpretedasa joint statevector, andI is ann n mmatrix wheremis

thenumberof rows in q.

Note that if the robot containsmotor inertia parametersthen motor inertia, referredto the link

referenceframe,will beaddedto thediagonalof M.

Example To show how the inertia ‘seen’ by the waist joint variesas a function of joint angles2 and 3 the

following codecouldbeused.

>> [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,:)));

−4−2

02

4

−4

−2

0

2

42

2.5

3

3.5

4

4.5

5

5.5

q2q3

I11

See Also robot,rne,itorque,coriolis,gravload


inertia 23




ishomog 24

ishomog

Purpose Testif argumentis a homogeneoustransformation

Synopsis ishomog(x)

Description Returnstrueif x is a 4 4 matrix.


itorque 25

itor que

Purpose Computethemanipulatorinertiatorquecomponent

Synopsis tau i = itorque(robot, q, qdd)

Description itorque returnsthejoint torquedueto inertiaat thespecifiedposeq andaccelerationqdd which is

givenby

τi M q q

If q andqdd arerow vectors,itorque is a row vectorof joint torques.If q andqdd arematrices,

eachrow is interpretedasa joint statevector, and itorque is a matrix in which eachrow is the

inertiatorquefor thecorrespondingrows of q andqdd .

robot is a robotobjectthatdescribesthekinematicsanddynamicsof themanipulatoranddrive. If

robot containsmotorinertiaparametersthenmotorinertia,referredto thelink referenceframe,will

beincludedin thediagonalof Mandinfluencetheinertiatorqueresult.

See Also robot,rne,coriolis, inertia,gravload


jacob0 26

jacob0

Purpose ComputemanipulatorJacobianin basecoordinates

Synopsis jacob0(robot, q)

Description jacob0 returnsa Jacobianmatrix for therobotobjectrobot in theposeq andasexpressedin the

basecoordinateframe.

The manipulatorJacobianmatrix, 0Jq, mapsdifferential velocities in joint space,q, to Cartesian

velocityof theend-effectorexpressedin thebasecoordinateframe.

0x 0Jq q q

For ann-axismanipulatortheJacobianis a 6 n matrix.

See Also jacobn,diff2tr, tr2diff, robot

References R. P. Paul, B. ShimanoandG. E. Mayer. KinematicControl Equationsfor SimpleManipulators.

IEEESystems,ManandCybernetics11(6),pp449-455,June1981.


jacobn 27

jacobn

Purpose ComputemanipulatorJacobianin end-effectorcoordinates

Synopsis jacobn(robot, q)

Description jacobn returnsa Jacobianmatrix for therobotobjectrobot in theposeq andasexpressedin the

end-effectorcoordinateframe.

The manipulatorJacobianmatrix, 0Jq, mapsdifferential velocities in joint space,q, to Cartesian

velocityof theend-effectorexpressedin theend-effectorcoordinateframe.

nx nJq q q

Therelationshipbetweentool-tip forcesandjoint torquesis givenby

τ nJq q nF

For ann-axismanipulatortheJacobianis a 6 n matrix.

See Also jacob0,diff2tr, tr2diff, robot

References R. P. Paul, B. ShimanoandG. E. Mayer. KinematicControl Equationsfor SimpleManipulators.

IEEESystems,ManandCybernetics11(6),pp449-455,June1981.


jtraj 28

jtraj

Purpose Computea joint spacetrajectorybetweentwo joint coordinateposes

Synopsis [q qd qdd] = jtraj(q0, q1, n)

[q qd qdd] = jtraj(q0, q1, n, qd0, qd1)

[q qd qdd] = jtraj(q0, q1, t)

[q qd qdd] = jtraj(q0, q1, t, qd0, qd1)

Description jtraj returnsa joint spacetrajectoryq from joint coordinatesq0 to q1 . Thenumberof pointsis n

or the lengthof thegiventime vectort . A 7th orderpolynomialis usedwith default zeroboundary

conditionsfor velocity andacceleration.

Non-zeroboundaryvelocitiescanbeoptionallyspecifiedasqd0 andqd1 .

The trajectoryis a matrix, with onerow per time step,andonecolumnper joint. The functioncan

optionallyreturna velocity andaccelerationtrajectoriesasqd andqdd respectively.

See Also ctraj


link 29

link

Purpose Link object

Synopsis 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 functionconstructsa link object. Theobjectcontainskinematicanddynamicparameters

aswell asactuatorand transmissionparameters.The first form returnsa default object,while the

secondandthird forms initialize thekinematicmodelbasedon Denavit andHartenberg parameters.

Thedynamicmodelcanbe initialized usingthe fourth form of theconstructorwheredyn row is a

1 20 matrix which is onerow of thelegacy dyn matrix.

By default thestandardDenavit andHartenberg conventionsareassumedbut this canbeoverridden

by theoptionalconvention argumentwhich canbesetto either’modified’ or ’standard’

(default). Notethatany abbreviation of thestringcanbeused,ie. ’mod’ or even ’m’ .

Thesecondlastform givenabove is not a constructorbut a link methodthatreturnsthelink transfor-

mationmatrix for the given joint coordinate.The argumentis given to the link objectusingparen-

thesis.Thesingleargumentis takenasthelink variableq andsubstitutedfor θ or D for a revoluteor

prismaticlink respectively.

TheDenavit andHartenberg parametersdescribethespatialrelationshipbetweenthis link andthepre-

viousone.Themeaningof thefieldsfor eachkinematicconventionaresummarizedin thefollowing

table.

variable DH MDH description

alpha αi αi 1 link twist angle

A Ai Ai 1 link length

theta θi θi link rotationangle

D Di Di link offsetdistance

sigma σi σi joint type;0 for revolute,non-zerofor prismatic

SinceMatlab doesnot supportthe conceptof public classvariablesmethodshave beenwritten to

allow link objectparametersto bereferenced(r) or assigned(a) asgivenby thefollowing table


link 30

Method Operations Returns

link .alpha r+a link twist angle

link .A r+a link length

link .theta r+a link rotationangle

link .D r+a link offsetdistance

link .sigma r+a joint type;0 for revolute,non-zerofor prismatic

link .RP r joint type;’R’ or ’P’

link .mdh r+a DH convention:0 if standard,1 if modified

link .I r 3 3 symmetricinertiamatrix

link .I a assignedfrom a3 3 matrixor a6-elementvec-

tor interprettedas IxxIyyIzzIxyIyzIxz

link .m r+a link mass

link .r r+a 3 1 link COGvector

link .G r+a gearratio

link .Jm r+a motorinertia

link .B r+a viscousfriction

link .Tc r Coulombfriction, 1 2 vectorwhere τ τlink .Tc a Coulombfriction; for symmetricfriction this is

ascalar, for asymmetricfriction it is a2-element

vectorfor positive andnegative velocity

link .dh r+a row of legacy DH matrix

link .dyn r+a row of legacy DYN matrix

link .qlim r+a joint coordinatelimits, 2-vector

link .islimit(q) r returntrueif valueof q is outsidethejoint limit

bounds

link .offset r+a joint coordinate offset (see discussion for

robot object).

Thedefault is for standardDenavit-Hartenberg conventions,zerofriction, massandinertias.

The display methodgives a one-linesummaryof the link’s kinematicparameters.The show

methoddisplaysasmany link parametersashave beeninitialized for thatlink.

Examples

>> L = link([-pi/2, 0.02, 0, 0.15])

L =

-1.570796 0.020000 0.000000 0.150000 R (std)

>> L.RP

ans =

R

>> L.mdh

ans =

0

>> L.G = 100;

>> L.Tc = 5;

>> L


link 31

L =

-1.570796 0.020000 0.000000 0.150000 R (std)

>> show(L)

alpha = -1.5708

A = 0.02

theta = 0

D = 0.15

sigma = 0

mdh = 0

G = 100

Tc = 5 -5

>>

Algorithm For thestandardDenavit-Hartenberg conventionsthehomogeneoustransform

i 1A i

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

0 sinαi cosαi di

0 0 0 1

representseachlink’s coordinateframewith respectto theprevious link’s coordinatesystem.For a

revolutejoint θi is offsetby

For themodifiedDenavit-Hartenberg conventionsit is instead

i 1A i

cosθi sinθi 0 ai 1

sinθi cosαi 1 cosθi cosαi 1 sinαi 1 di sinαi 1

sinθi sinαi 1 cosθi sinαi 1 cosαi 1 di cosαi 1

0 0 0 1

See Also showlink, robot

References R. P. Paul. RobotManipulators: Mathematics,Programming, andControl. MIT Press,Cambridge,

Massachusetts,1981.

J.J.Craig,Introductionto Robotics. AddisonWesley, seconded.,1989.


maniplty 32

maniplty

Purpose Manipulabilitymeasure

Synopsis m = maniplty(robot, q)

m = maniplty(robot, q, which)

Description maniplty computesthescalarmanipulabilityindex for themanipulatorat thegivenpose.Manipu-

lability variesfrom 0 (bad)to 1 (good).robot is arobotobjectthatcontainskinematicandoptionally

dynamicparametersfor themanipulator. Two measuresaresupportedandareselectedby theoptional

argumentwhich canbeeither ’yoshikawa’ (default) or ’asada’ . Yoshikawa’s manipulability

measureis basedpurely on kinematicdata,andgivesan indicationof how ‘f ar’ the manipulatoris

from singularitiesandthusableto move andexert forcesuniformly in all directions.

Asada’smanipulabilitymeasureutilizesmanipulatordynamicdata,andindicateshow closetheinertia

ellipsoidis to spherical.

If q is avectormaniplty returnsascalarmanipulabilityindex. If q is amatrixmaniplty returns

a columnvectorandeachrow is themanipulabilityindex for theposespecifiedby thecorresponding

row of q.

Algorithm Yoshikawa’s measureis basedon theconditionnumberof themanipulatorJacobian

ηyoshi J q J q

Asada’s measureis computedfrom theCartesianinertiamatrix

M x J q TM q J q 1

TheCartesianmanipulatorinertiaellipsoidis

x M x x 1

andgivesanindicationof how well themanipulatorcanacceleratein eachof theCartesiandirections.

Thescalarmeasurecomputedhereis theratio of thesmallest/largestellipsoidaxes

ηasadaminxmaxx

Ideally theellipsoidwould bespherical,giving a ratioof 1, but in practicewill belessthan1.

See Also jacob0,inertia,robot

References T. Yoshikawa, “Analysisandcontrolof robotmanipulatorswith redundancy,” in Proc. 1stInt. Symp.

RoboticsResearch, (BrettonWoods,NH), pp.735–747,1983.


robot/nofriction 33

robot/nofriction

Purpose Remove friction from robotobject

Synopsis robot2 = nofriction(robot)

robot2 = nofriction(robot, ’all’)

Description Returna new robot objectwith modified joint friction properties.The first form setsthe Coulomb

friction valuesto zero in the constituentlinks The secondform setsviscousandCoulombfriction

valuesin theconstituentlinks aresetto zero.

Theresultingrobotobjecthasits namestringprependedwith ’NF/’.

This is importantfor forward dynamicscomputation(fdyn() ) wherethe presenceof friction can

preventthenumericalintegrationfrom converging.

See Also link/nofriction, robot,link, friction, fdyn


link/nofriction 34

link/nofriction

Purpose Remove friction from link object

Synopsis link2 = nofriction(link)

link2 = nofriction(link, ’all’)

Description Returna new link object with modified joint friction properties. The first form setsthe Coulomb

friction valuesto zero.Thesecondform setsbothviscousandCoulombfriction valuesto zero.

This is importantfor forward dynamicscomputation(fdyn() ) wherethe presenceof friction can

preventthenumericalintegrationfrom converging.

See Also robot/nofriction,link, friction, fdyn


oa2tr 35

oa2tr

Purpose Convert OA vectorsto homogeneoustransform

Synopsis oa2tr(o, a)

Description oa2tr returnsa rotationalhomogeneoustransformationspecifiedin termsof theCartesianorienta-

tion andapproachvectorso anda respectively.

Algorithm

To a o a 0

0 0 0 1

whereo anda areunit vectorscorrespondingto o anda respectively.

See Also rpy2tr, eul2tr


perturb 36

perturb

Purpose Perturbrobotdynamicparameters

Synopsis robot2 = perturb(robot, p)

Description Returna new robotobjectwith randomlymodifieddynamicparameters:link massandinertia. The

perturbationis multiplicative sothatvaluesaremultiplied by randomnumbersin theinterval (1-p) to

(1+p).

Useful for investigatingthe robustnessof variousmodel-basedcontrol schemeswhereone model

formsthebasisof themodel-basedcontrollerandthepeturbedmodelis usedfor theactualplant.

Theresultingrobotobjecthasits namestringprependedwith ’P/’.

See Also fdyn, rne,robot


puma560 37

puma560

Purpose Createa Puma560robotobject

Synopsis puma560

Description Createsthe robot objectp560 which describesthekinematicanddynamiccharacteristicsof a Uni-

mationPuma560 manipulator. The kinematicconventionsusedareasper Paul andZhang,andall

quantitiesarein standardSI units.

Also definesthe joint coordinatevectorsqz , qr andqstretch correspondingto the zero-angle,

readyandfully extended(in X-direction)posesrespectively.

Detailsof coordinateframesusedfor thePuma560shown herein its zeroanglepose.

See Also robot,puma560akb,stanford

References R. P. Paul and H. Zhang, “Computationallyefficient kinematicsfor manipulatorswith spherical

wrists,” Int. J. Robot.Res., vol. 5, no.2, pp.32–44,1986.

P. Corke andB. Armstrong-Helouvry, “A searchfor consensusamongmodelparametersreportedfor

thePUMA 560 robot,” in Proc. IEEE Int. Conf. RoboticsandAutomation, (SanDiego), pp. 1608–

1613,May 1994.

P. Corke andB. Armstrong-Helouvry, “A meta-studyof PUMA 560dynamics:A critical appraisalof

literaturedata,” Robotica, vol. 13,no.3, pp.253–258,1995.


puma560akb 38

puma560akb

Purpose Createa Puma560robotobject

Synopsis puma560akb

Description Createstherobot objectp560m whichdescribesthekinematicanddynamiccharacteristicsof aUni-

mationPuma560manipulator. It usesCraig’s modifiedDenavit-Hartenberg notationwith thepartic-

ular kinematicconventionsfrom Armstrong,Khatib andBurdick. All quantitiesare in standardSI

units.

Also definesthe joint coordinatevectorsqz , qr andqstretch correspondingto the zero-angle,

readyandfully extended(in X-direction)posesrespectively.

See Also robot,puma560,stanford

References B. Armstrong,O. Khatib, andJ. Burdick, “The explicit dynamicmodelandinertial parametersof

thePuma560arm,” in Proc. IEEE Int. Conf. RoboticsandAutomation, vol. 1, (Washington,USA),

pp.510–18,1986.


qinterp 39

qinterp

Purpose Interpolateunit-quaternions

Synopsis QI = qinterp(Q1, Q2, r)

Description Returnaunit-quaternionthatinterpolatesbetweenQ1andQ2asr variesbetween0 and1 inclusively.

This is a sphericallinear interpolation(slerp) that canbe interpretedas interpolationalonga great

circlearcon asphere.

If r is a vector, thena cell arrayof quaternionsis returnedcorrespondingto successive valuesof r .

Examples A simpleexample

>> 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 quaternioncurves.,” in Proceedingsof ACM SIGGRAPH,

(SanFrancisco),pp.245–254,TheSingerCompany, Link Flight SimulatorDivision,1985.


quaternion 40

quaternion

Purpose Quaternionobject

Synopsis q = quaternion(qq)

q = quaternion(v, theta)

q = quaternion(R)

q = quaternion([s vx vy vz])

Description quaternion is theconstructorfor aquaternion object.Thefirst form returnsanew objectwith the

samevalueasits argument.Thesecondform initializesthequaternionto a rotationof theta about

thevectorv .

Examples A simpleexample

>> 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 setsthequaternionto a rotationequivalent to the given 3 3 rotationmatrix, or the

rotationsubmatrixof a 4 4 homogeneoustransform.

Thefourth form setsthefour quaternionelementsdirectly wheres is thescalarcomponentand[vx

vy vz] thevector.

All forms,exceptthelast,returna unit quaternion,ie. onewhosemagnitudeis unity.

Someoperatorsareoverloadedfor thequaternionclass


quaternion 41

q1 * q2 returnsquaternionproductor compounding

q * v returnsaquaternionvectorproduct,thatis thevectorv is rotated

by thequaternion.v is a 3 3 vector

q1 / q2 returnsq1 q 12

q j returnsqj where j is an integerexponent.For j 0 theresult

is obtainedby repeatedmultiplication.For j 0 thefinal result

is inverted.

double(q) returnsthequaternioncoeffientsasa 4-elementrow vector

inv(q) returnsthequaterioninverse

norm(q) returnsthequaterionmagnitude

plot(q) displaysa 3D plot showing thestandardcoordinateframeafter

rotationby q.

unit(q) returnsthecorrespondingunit quaterion

Somepublic classvariablesmethodsarealsoavailablefor referenceonly.

method Returns

quaternion .d return4-vectorof quaternionelements

quaternion .s returnscalarcomponent

quaternion .v returnvectorcomponent

quaternion .t returnequivalenthomogeneoustransformation

matrix

quaternion .r returnequivalentorthonormalrotationmatrix

Examples

>> t = rotx(0.2)

t =

1.0000 0 0 0

0 0.9801 -0.1987 0

0 0.1987 0.9801 0

0 0 0 1.0000

>> q1 = quaternion(t)

q1 =

0.995 <0.099833, 0, 0>

>> q1.r

ans =

1.0000 0 0

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 =


quaternion 42

0.98383 <0.098712, 0.14869, 0.014919>

>> q1*q1

ans =

0.98007 <0.19867, 0, 0>

>> q1ˆ2

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>

Cautionar y At themomentvectorsor arraysof quaternionsarenotsupported.Youcanhowever usecell arraysto

holda numberof quaternions.

See Also quaternion/plot

References K. Shoemake, “Animating rotation with quaternioncurves.,” in Proceedingsof ACM SIGGRAPH,

(SanFrancisco),pp.245–254,TheSingerCompany, Link Flight SimulatorDivision,1985.


quaternion/plot 43

quaternion/plot

Purpose Plotquaternionrotation

Synopsis plot(Q)

Description plot is overloadedfor quaternion objectsanddisplaysa 3D plot which shows how the standard

axesaretransformedunderthatrotation.

Examples A rotationof 0.3radabouttheX axis.ClearlytheX axisis invariantunderthis rotation.

>> q=quaternion(rotx(0.3))

q =

0.85303<0.52185, 0, 0>

>> plot(q)

−1

−0.5

0

0.5

1

−1

−0.5

0

0.5

1−1

−0.5

0

0.5

1

X

X

Z

Y

Y

Z

See Also quaternion


rne 44

rne

Purpose Computeinversedynamicsvia recursive Newton-Eulerformulation

Synopsis 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 computestheequationsof motionin anefficientmanner, giving joint torqueasafunctionof joint

position,velocityandacceleration.

If q, qd andqdd arerow vectorsthen tau is a row vectorof joint torques.If q, qd andqdd are

matricesthentau is a matrix in which eachrow is thejoint torquefor thecorrespondingrows of q,

qd andqdd .

Gravity directionis definedby therobotobjectbut maybeoverriddenby providing agravity acceler-

ationvectorgrav = [gx gy gz] .

An externalforce/momentactingon theendof themanipulatormayalsobespecifiedby a 6-element

vectorfext = [Fx Fy Fz Mx My Mz] in theend-effectorcoordinateframe.

Thetorquecomputedmaycontaincontributionsdueto armatureinertiaandjoint friction if theseare

specifiedin theparametermatrix dyn .

TheMEX-file versionof this functionis over 1000timesfasterthantheM-file. SeeSection1 of this

manualfor informationabouthow to compileandinstall theMEX-file.

Algorithm Coumputesthejoint torque

τ M q q C q q q F q G q

whereM is themanipulatorinertiamatrix,C is theCoriolis andcentripetaltorque,F theviscousand

Coulombfriction, andG thegravity load.

Cautionar y TheMEX file currentlyignoressupportbaseandtool transforms.

See Also robot,fdyn, accel,gravload,inertia,friction

Limitations A MEX file is currentlyonly availablefor Sparcarchitecture.


rne 45

References J.Y. S.Luh, M. W. Walker, andR. P. C. Paul. On-linecomputationalschemefor mechanicalmanip-

ulators.ASMEJournalof DynamicSystems,MeasurementandControl, 102:69–76,1980.


robot 46

robot

Purpose Robotobject

Synopsis r = robot

r = robot(rr)

r = robot(link ...)

r = robot(DH ...)

r = robot(DYN ...)

Description robot is the constructorfor a robot object. The first form createsa default robot, andthe second

form returnsa new robotobjectwith thesamevalueasits argument.The third form createsa robot

from a cell arrayof link objectswhich definethe robot’s kinematicsandoptionally dynamics.The

fourthandfifth formscreatea robotobjectfrom legacy DH andDYN formatmatrices.

The last threeforms all acceptoptionaltrailing string argumentswhich aretaken in orderasbeing

robotname,manufacturerandcomment.

SinceMatlab doesnot supportthe conceptof public classvariablesmethodshave beenwritten to

allow robotobjectparametersto bereferenced(r) or assigned(a)asgivenby thefollowing table

method Operation Returns

robot .n r numberof joints

robot .link r+a cell arrayof link objects

robot .name r+a robotnamestring

robot .manuf r+a robotmanufacturerstring

robot .comment r+a generalcommentstring

robot .gravity r+a 3-elementvectordefininggravity direction

robot .mdh r DH convention: 0 if standard,1 if modified.

Determinedfrom thelink objects.

robot .base r+a homogeneoustransformdefiningbaseof robot

robot .tool r+a homogeneoustransformdefiningtool of robot

robot .dh r legacy DH matrix

robot .dyn r legacy DYN matrix

robot .q r+a joint coordinates

robot .qlim r+a joint coordinatelimits, n 2 matrix

robot .islimit r joint limit vector, for eachjoint set to -1, 0 or

1 dependingif below low limit, OK, or greater

thanupperlimit

robot .offset r+a joint coordinateoffsets

robot .plotopt r+a optionsfor plot()

robot .lineopt r+a line stylefor robotgraphicallinks

robot .shadowopt r+a line stylefor robotshadow links

robot .handle r+a graphicshandles

Someof theseoperationsat therobot level areactuallywrappersaroundsimilarly namedlink object


robot 47

functions:offset , qlim , islimit .

Theoffsetvectoris addedto theuserspecifiedjoint anglesbeforeany kinematicor dynamicfunctionis

invoked(it is actuallyimplementedwithin thelink object).Similarly it is subtractedafteranoperation

suchas inversekinematics. The needfor a joint offset vectorarisesbecauseof the constraintsof

theDenavit-Hartenberg (or modifiedDenavit-Hartenberg) notation.Theposeof therobotwith zero

joint anglesis frequentlysomeratherunusual(or evenunachievable)pose.Thejoint coordinateoffset

providesa meansto make anarbitraryposecorrespondto thezerojoint anglecase.

Default valuesfor robotparametersare:

robot.name ’noname’

robot.manuf ”

robot.comment ”

robot.gravity 009 81 m s2

robot.offset ones(n,1)

robot.base eye(4,4)

robot.tool eye(4,4)

robot.lineopt ’Color’, ’black’, ’Linewidth’, 4

robot.shadowopt ’Color’, ’black’, ’Linewidth’, 1

Themultiplicationoperator, * , is overloadedandtheproductof two robotobjectsis a robotwhich is

theseriesconnectionof themultiplicands.Tool transformsof all but thelast robotareignored,base

transformof all but thefirst robotareignored.

Theplot functionis alsooverloadedandis usedto provide arobotanimation.

Examples

>> L1 = link([ pi/2 0 0 0])

L =

[1x1 link]

>> L2 = 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 A theta D R/P

1.570796 0.000000 0.000000 0.000000 R (std)

0.000000 0.000000 0.500000 0.000000 R (std)


robot 48

>>

See Also link,plot


robot/plot 49

robot/plot

Purpose Graphicalrobotanimation

Synopsis plot(robot, q)

plot(robot, q, arguments...)

−1

−0.5

0

0.5

1

−1

−0.8

−0.6

−0.4

−0.2

0

0.2

0.4

0.6

0.8

1

−1

−0.8

−0.6

−0.4

−0.2

0

0.2

0.4

0.6

0.8

1

Puma 560

xyz

Description plot is overloadedfor robot objectsanddisplaysa graphicalrepresentationof the robotgiven the

kinematicinformationin robot . The robot is representedby a simplestick figure polyline where

line segmentsjoin theoriginsof thelink coordinateframes.If q is amatrix representinga joint-space

trajectorythenananimationof therobotmotionis shown.

GRAPHICAL ANNOTATIONS

Thebasicstickfigurerobotcanbeannotatedwith

shadow on the‘floor’

XYZ wrist axes and labels,shown by 3 short orthogonalline segmentswhich are colored:

red(X or normal),green(Y or orientation)andblue(Z or approach).They canbeoptionally

labelledXYZ or NOA.

joints, theseare3D cylindersfor revolutejoints andboxesfor prismaticjoints

therobot’s name

All of theserequiresomekind of dimensionand this is determinedusinga simpleheuristicfrom


robot/plot 50

theworkspacedimensions.This dimensioncanbechangedby settingthemultiplicative scalefactor

usingthemag optionbelow. Thesevariousannotationsdo slow therateat which animationswill be

rendered.

OPTIONS

Optionsarespecifiedby a variablelengthargumentlist comprisingstringsandnumericvalues.The

allowedvaluesare:

workspace w setthe3D plot boundsor workspaceto thematrix [xmin xmax ymin ymax

zmin zmax]

perspective show a perspective view

ortho show anorthogonalview

base, nobase controldisplayof base,a line from thefloor uptojoint 0

wrist, nowrist controldisplayof wrist axes

name, noname controldisplayof robotnamenearjoint 0

shadow, noshadow controldisplayof a ’shadow’ on thefloor

joints, nojoints control display of joints, theseare cylinders for revolute joints and boxes for

prismaticjoints

xyz wrist axislabelsareX, Y, Z

noa wrist axislabelsareN, O, A

mag scale annotationscalefactor

erase, noerase controlerasureof robotaftereachchange

loop, noloop controlwhetheranimationis repeatedendlessly

Theoptionscomefrom 3 sourcesandareprocessedin theorder:

1. Cell arrayof optionsreturnedby the function PLOTBOTOPTif found on the user’s current

path.

2. Cell arrayof optionsreturnedby the .plotopt methodof the robot object. Theseareset

by the.plotopt method.

3. List of argumentsin thecommandline.

GETTING GRAPHICAL ROBOT STATE

Eachgraphicalrobothasa uniquetagsetequalto therobot’sname.Whenplot is calledit looksfor

all graphicalobjectswith thatnameandmovesthem.Thegraphicalrobotholdsacopy of therobot

objectasUserData . Thatcopy containsthegraphicalhandlesof all thegraphicalsub-elementsof

therobotandalsothecurrentjoint anglestate.

This stateis used,andadjusted,by the drivebot function. The currentjoint anglestatecanbe

obtainedby q = plot(robot) . If multiple instancesexist, thatof thefirst onereturnedby find-

obj() is given.

Examples To animatetwo Pumasmoving in thesamefigurewindow.

>> clf

>> p560b = p560; % duplicate the robot

>> p560b.name = ’Another Puma 560’; % give it a unique name

>> p560b.base = transl([-.05 0.5 0]); % move its base


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 thesamerobot.

>> clf

>> figure % create a new figure

>> plot(p560, qz); % add a graphical robot

>> figure % create another figure

>> plot(p560, qz); % add a graphical robot

>> plot(p560, qr); % both robots should move

Now thetwo figurescanbeadjustedto give differentviewpoints,for instance,planandelevation.

Cautionar y plot() optionsareonly processedon thefirst call whenthegraphicalobjectis established,they are

skippedonsubsequentcalls.Thusif you wish to changeoptions,clearthefigurebeforereplotting.

See Also drivebot,fkine, robot


rotvec 52

rotvec

Purpose Rotationabouta vector

Synopsis T = rotvec(v, theta)

Description rotvec returnsa homogeneoustransformationrepresentinga rotationof theta radiansaboutthe

vectorv .

See Also rotx, roty, rotz


rotx,roty,rotz 53

rotx,roty,rotz

Purpose RotationaboutX, Y or Z axis

Synopsis T = rotx(theta)

T = roty(theta)

T = rotz(theta)

Description Returna homogeneoustransformationrepresentinga rotationof theta radiansabouttheX, Y or Z

axes.

See Also rotvec


rpy2tr 54

rpy2tr

Purpose Roll/pitch/yaw anglesto homogeneoustransform

Synopsis T = rpy2tr([r p y])

T = rpy2tr(r,p,y)

Description rpy2tr returnsa homogeneoustransformationfor thespecifiedroll/pitch/yaw anglesin radians.

T RZ r RY p RX y

See Also tr2rpy, eul2tr

References R. P. Paul, Robot Manipulators: Mathematics,Programming, and Control. Cambridge,Mas-

sachusetts:MIT Press,1981.


rtdemo 55

rtdemo

Purpose RobotToolboxdemonstration

Synopsis rtdemo

Description Thisscriptprovidesdemonstrationsfor mostfunctionswithin theRoboticsToolbox.

Cautionar y Thisscriptclearsall variablesin theworkspaceanddeletesall figures.


showlink 56

showlink

Purpose Show robotlink details

Synopsis showlink(robot)

showlink(link)

Description Displaysin detailall theparameters,includingall definedinertialparameters,of a link. Thefirst form

providesthis level of detailfor all links in thespecifiedmanipulator. roll/pitch/yaw anglesin radians.

Examples To show detailsof Pumalink 2

>> showlink(p560.link2)

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 0.524 0

0 0 0.539

Jm = 0.0002

G = 107.815

B = 0.000817

Tc = 0.126 -0.071

qlim =

>>


stanford 57

stanford

Purpose Createa Stanfordmanipulatorrobotobject

Synopsis stanford

−2−1

01

2

−2

−1

0

1

2−2

−1

0

1

2

XY

Z

Stanford armx

yz

Description Createstherobot objectstan which describesthekinematicanddynamiccharacteristicsof a Stan-

ford manipulator. Specifiesarmatureinertiaandgearratios.All quantitiesarein standardSI units.

See Also robot,puma560

References R. Paul, “Modeling, trajectorycalculationandservoing of a computercontrolledarm,” Tech. Rep.

AIM-177, StanfordUniversity, Artificial IntelligenceLaboratory, 1972.

R. P. Paul, Robot Manipulators: Mathematics,Programming, and Control. Cambridge,Mas-



tr2diff 58

tr2diff

Purpose Convert a homogeneoustransformto a differentialmotionvector

Synopsis d = tr2diff(T)

d = tr2diff(T1, T2)

Description Thefirst form of tr2diff returnsa6-elementdifferentialmotionvectorrepresentingtheincremental

translationandrotationdescribedby thehomogeneoustransformT. It is assumedthatT is of theform

0 δz δy dx

δz 0 δx dy

δy δx 0 dz

0 0 0 0

Thetranslationalelementsof d areassigneddirectly. Therotationalelementsarecomputedfrom the

meanof thetwo valuesthatappearin theskew-symmetricmatrix.

The secondform of tr2diff returnsa 6-elementdifferentialmotion vector representingthe dis-

placementfrom T1 to T2, thatis, T2 - T1.

dp

2p

11 2 n1 n2 o1 o2 a1 a2

See Also diff2tr




tr2eul 59

tr2eul

Purpose Convert a homogeneoustransformto Eulerangles

Synopsis [a b c] = tr2eul(T)

Description tr2eul returnsavectorof Eulerangles,in radians,correspondingto therotationalpartof thehomo-

geneoustransformT.

Trot RZ a RY b RZ c

Cautionar y Notethat12 differentEuleranglesetsor conventionsexist. Theconventionusedhereis thecommon

onefor robotics,but is not theoneusedfor examplein theaerospacecommunity.

See Also eul2tr, tr2rpy




tr2jac 60

tr2jac

Purpose Computea Jacobianto mapdifferentialmotionbetweenframes

Synopsis jac = tr2jac(T)

Description tr2jac returnsa 6 6 Jacobianmatrix to mapdifferentialmotionsor velocitiesbetweenframes

relatedby thehomogeneoustransformT.

If T representsa homogeneoustransformationfrom frameA to frameB, ATB, then

Bx BJAAx

whereBJA is givenby tr2jac(T) .

See Also tr2diff, diff2tr




tr2rpy 61

tr2r py

Purpose Convert a homogeneoustransformto roll/pitch/yaw angles

Synopsis [a b c] = tr2rpy(T)

Description tr2rpy returnsa vectorof roll/pitch/yaw angles,in radians,correspondingto therotationalpartof

thehomogeneoustransformT

Trot RZ r RY p RX y

See Also rpy2tr, tr2eul




transl 62

transl

Purpose Translationaltransformation

Synopsis T = transl(x, y, z)

T = transl(v)

v = transl(T)

xyz = transl(TC)

Description Thefirst two formsreturnahomogeneoustransformationrepresentingatranslationexpressedasthree

scalarx , y andz , or a Cartesianvectorv .

Thethird form returnsthetranslationalpartof ahomogeneoustransformasa3-elementcolumnvector.

Thefourthform returnsamatrixwhosecolumnsaretheX, Y andZ columnsof the4 4 mCartesian

trajectorymatrixTC.

See Also ctraj


trinterp 63

trinter p

Purpose Interpolatehomogeneoustransforms

Synopsis T = trinterp(T0, T1, r)

Description trinterp interpolatesbetweenthetwo homogeneoustransformsT0 andT1 asr variesbetween0

and1 inclusively. This is generallyusedfor computingstraightline or ‘Cartesian’motion.Rotational

interpolationis achievedusingquaternionsphericallinearinterpolation.

Examples Interpolationof homogeneoustransformations.

>> 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 1.0000

0 1.0000 0 4.0000

-0.2955 0 0.9553 5.0000

0 0 0 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) % should be t2

ans =

0.9553 0 0.2955 1.0000

0 1.0000 0 4.0000

-0.2955 0 0.9553 5.0000

0 0 0 1.0000


trinterp 64

>> trinterp(t1,t2,0.5) % ’half way’ in between

ans =

0.9887 0.0075 0.1494 0.5000

0.0075 0.9950 -0.0998 2.0000

-0.1494 0.0998 0.9837 2.5000

0 0 0 1.0000

>>

See Also ctraj,qinterp




trnorm 65

tr norm

Purpose Normalizeahomogeneoustransformation

Synopsis TN = trnorm(T)

Description Returnsa normalizedcopy of thehomogeneoustransformationT. Finite word lengtharithmeticcan

lead to homogeneoustransformationsin which the rotationalsubmatrixis not orthogonal,that is,

det R 1.

Algorithm Normalizationis performedby orthogonalizingtherotationsubmatrixn o a.

See Also oa2tr

References J.Funda,“Quaternionsandhomogeneoustransformsin robotics,” Master’sthesis,Universityof Penn-

sylvania,Apr. 1988.


twolink 66

twolink

Purpose Loadkinematicanddynamicdatafor a simple2-link mechanism

Synopsis twolink

−2−1

01

2

−2

−1

0

1

2−2

−1

0

1

2

XY

Z

Simple two link

xy z

Description Createsthe robot object tl which describesthe kinematicanddynamiccharacteristicsof a simple

two-link planarmanipulator. Themanipulatoroperatesin thehorizontal(XY) planeandis therefore

not influencedby gravity.

Massis assumedto beconcentratedat thejoints. All massesandlengthsareunity.

See Also puma560,stanford

References Fig 3-6of “Robot DynamicsandControl” by M.W. SpongandM. Vidyasagar, 1989.


unit 67

unit

Purpose Unitizea vector

Synopsis vn = unit(v)

Description unit returnsa unit vectoralignedwith v .

Algorithmvn

vv


dh(legacy) 68

dh (legacy)

Purpose Matrix representationof manipulatorkinematics

Description A dh matrix describesthekinematicsof a manipulatorin a generalway usingthestandardDenavit-

Hartenberg conventions.Eachrow representsonelink of themanipulatorandthecolumnsareassigned

accordingto thefollowing table.

Column Symbol Description

1 α i link twist angle

2 Ai link length

3 θi link rotationangle

4 Di link offsetdistance

5 σi joint type;0 for revolute,non-zerofor prismatic

If the lastcolumnis not given,toolbox functionsassumethat themanipulatoris all-revolute. For an

n-axismanipulatordh is ann 4 or n 5 matrix.

Thefirst 5 columnsof a dyn matrix containthekinematicparametersandmaybeusedanywherethat

adh kinematicmatrix is required— thedynamicdatais ignored.

LengthsAi andDi may be expressedin any unit, andthis choicewill flow on to the units in which

homogeneoustransformsandJacobiansarerepresented.All anglesarein radians.

See Also puma560,stanford,mdh




dyn (legacy) 69

dyn (legacy)

Purpose Matrix representationof manipulatorkinematicsanddynamics

Description A dyn matrix describesthe kinematicsanddynamicsof a manipulatorin a generalway using the

standardDenavit-Hartenberg conventions. Eachrow representsonelink of the manipulatorandthe

columnsareassignedaccordingto thefollowing table.

Column Symbol Description

1 α link twist angle

2 A link length

3 θ link rotationangle

4 D link offsetdistance

5 σ joint type;0 for revolute,non-zerofor prismatic

6 mass massof thelink

7 rx link COGwith respectto thelink coordinateframe

8 ry

9 rz

10 Ixx elementsof link inertiatensoraboutthelink COG

11 Iyy

12 Izz

13 Ixy

14 Iyz

15 Ixz

16 Jm armatureinertia

17 G reductiongearratio; joint speed/linkspeed

18 B viscousfriction, motorreferred

19 Tc+ coulombfriction (positive rotation),motorreferred

20 Tc- coulombfriction (negative rotation),motorreferred

For an n-axismanipulator, dyn is an n 20 matrix. The first 5 columnsof a dyn matrix contain

the kinematicparametersandmaybeusedanywherethat a dh kinematicmatrix is required— the

dynamicdatais ignored.

All anglesarein radians.Thechoiceof all otherunitsis upto theuser, andthischoicewill flow on to

theunitsin which homogeneoustransforms,Jacobians,inertiasandtorquesarerepresented.

See Also dh


robotics toolbox - computer engineering group - · pdf filefigure 4: robotics toolbox example...

Documents