Upload
buinhan
View
262
Download
3
Embed Size (px)
Citation preview
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
MDH: Speedup is 1509, worst case error is 0.000000
************************ ext force *****************************
DH: Speedup is 1497, worst case error is 0.000000
MDH: Speedup is 637, worst case error is 0.000000
***************************************************************
********************** Stanford arm ***************************
***************************************************************
************************ normal case *****************************
DH: Speedup is 1490, worst case error is 0.000000
MDH: Speedup is 1519, worst case error is 0.000000
************************ no gravity *****************************
DH: Speedup is 1471, worst case error is 0.000000
MDH: Speedup is 1450, worst case error is 0.000000
************************ ext force *****************************
DH: Speedup is 417, worst case error is 0.000000
MDH: Speedup is 1458, worst case error is 0.000000
>>
This will run 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.
To runthisdemofirst createarobotobjectin theworkspace,typicallyby usingthepuma560
command,thenstartthesimulationusingSimulation/Startoptionfrom themodeltoolbar.
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).
To runthisdemofirst createarobotobjectin theworkspace,typicallyby usingthepuma560
command,thenstartthesimulationusingSimulation/Startoptionfrom themodeltoolbar.
>> 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 .
To runthisdemofirst createarobotobjectin theworkspace,typicallyby usingthepuma560
command,thenstartthesimulationusingSimulation/Startoptionfrom themodeltoolbar.
>> 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.
4 MANIPULATORRIGID-BODY DYNAMICS 19
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].
4 MANIPULATORRIGID-BODY DYNAMICS 20
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
4 MANIPULATORRIGID-BODY DYNAMICS 21
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
RoboticsToolboxRelease7 PeterCorke,April 2002
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
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
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
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
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);
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
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
RoboticsToolboxRelease7 PeterCorke,April 2002
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
RoboticsToolboxRelease7 PeterCorke,April 2002
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
RoboticsToolboxRelease7 PeterCorke,April 2002
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
References M. W. Walker andD. E. Orin. Efficientdynamiccomputersimulationof roboticmechanisms.ASME
Journalof DynamicSystems,MeasurementandControl, 104:205–211,1982.
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
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]
RoboticsToolboxRelease7 PeterCorke,April 2002
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
RoboticsToolboxRelease7 PeterCorke,April 2002
inertia 23
References M. W. Walker andD. E. Orin. Efficientdynamiccomputersimulationof roboticmechanisms.ASME
Journalof DynamicSystems,MeasurementandControl, 104:205–211,1982.
RoboticsToolboxRelease7 PeterCorke,April 2002
ishomog 24
ishomog
Purpose Testif argumentis a homogeneoustransformation
Synopsis ishomog(x)
Description Returnstrueif x is a 4 4 matrix.
RoboticsToolboxRelease7 PeterCorke,April 2002
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
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
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
RoboticsToolboxRelease7 PeterCorke,April 2002
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
RoboticsToolboxRelease7 PeterCorke,April 2002
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
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
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
RoboticsToolboxRelease7 PeterCorke,April 2002
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
RoboticsToolboxRelease7 PeterCorke,April 2002
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
RoboticsToolboxRelease7 PeterCorke,April 2002
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
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
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
RoboticsToolboxRelease7 PeterCorke,April 2002
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 =
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
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
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
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
RoboticsToolboxRelease7 PeterCorke,April 2002
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)
RoboticsToolboxRelease7 PeterCorke,April 2002
robot 48
>>
See Also link,plot
RoboticsToolboxRelease7 PeterCorke,April 2002
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
RoboticsToolboxRelease7 PeterCorke,April 2002
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
RoboticsToolboxRelease7 PeterCorke,April 2002
robot/plot 51
>> plot(p560, qr); % display it at ready position
>> hold on
>> plot(p560b, qr); % display it at ready position
>> t = [0:0.056:10];
>> jt = jtraj(qr, qstretch, t); % trajectory to stretch pose
>> for q = jt’, % for all points on the path
>> plot(p560, q);
>> plot(p560b, q);
>> end
To show multiple views of 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
RoboticsToolboxRelease7 PeterCorke,April 2002
rotvec 52
rotvec
Purpose Rotationabouta vector
Synopsis T = rotvec(v, theta)
Description rotvec returnsa homogeneoustransformationrepresentinga rotationof theta radiansaboutthe
vectorv .
See Also rotx, roty, rotz
RoboticsToolboxRelease7 PeterCorke,April 2002
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
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
rtdemo 55
rtdemo
Purpose RobotToolboxdemonstration
Synopsis rtdemo
Description Thisscriptprovidesdemonstrationsfor mostfunctionswithin theRoboticsToolbox.
Cautionar y Thisscriptclearsall variablesin theworkspaceanddeletesall figures.
RoboticsToolboxRelease7 PeterCorke,April 2002
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 =
>>
RoboticsToolboxRelease7 PeterCorke,April 2002
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-
sachusetts:MIT Press,1981.
RoboticsToolboxRelease7 PeterCorke,April 2002
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
References R. P. Paul, Robot Manipulators: Mathematics,Programming, and Control. Cambridge,Mas-
sachusetts:MIT Press,1981.
RoboticsToolboxRelease7 PeterCorke,April 2002
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
References R. P. Paul, Robot Manipulators: Mathematics,Programming, and Control. Cambridge,Mas-
sachusetts:MIT Press,1981.
RoboticsToolboxRelease7 PeterCorke,April 2002
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
References R. P. Paul, Robot Manipulators: Mathematics,Programming, and Control. Cambridge,Mas-
sachusetts:MIT Press,1981.
RoboticsToolboxRelease7 PeterCorke,April 2002
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
References R. P. Paul, Robot Manipulators: Mathematics,Programming, and Control. Cambridge,Mas-
sachusetts:MIT Press,1981.
RoboticsToolboxRelease7 PeterCorke,April 2002
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
RoboticsToolboxRelease7 PeterCorke,April 2002
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
RoboticsToolboxRelease7 PeterCorke,April 2002
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
References R. P. Paul, Robot Manipulators: Mathematics,Programming, and Control. Cambridge,Mas-
sachusetts:MIT Press,1981.
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
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.
RoboticsToolboxRelease7 PeterCorke,April 2002
unit 67
unit
Purpose Unitizea vector
Synopsis vn = unit(v)
Description unit returnsa unit vectoralignedwith v .
Algorithmvn
vv
RoboticsToolboxRelease7 PeterCorke,April 2002
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
References R. P. Paul, Robot Manipulators: Mathematics,Programming, and Control. Cambridge,Mas-
sachusetts:MIT Press,1981.
RoboticsToolboxRelease7 PeterCorke,April 2002
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
RoboticsToolboxRelease7 PeterCorke,April 2002