ADAM BIENKOWSKI,

KRZYSZTOF KOZLOWSKI



Chair of Control, Robotics, and Computer Science 

Poznan University of Technology 

ul. Piotrowo 3a, 60-965 Poznan, POLAND 

e-mail 

bienkows@ar-kari.put.poznan.pl.,

kk@ar-kari.put.poznan.pl.,



Numerical calculations for robotics in Mathematica

Calculations method

I am from Poznan University of Technology in Poland. 

We normally work with robots. In Mathematica we solved a general robotic 
problems. In our package we have implemented kinematical and dynamical 
algorithms. 

Package is called Robo. We use it to teach and research. 

To solve robotic problem we use Newton-Euler method for forward dynamics and 
Step-Descent for inverse kinematics. For other problem we don't use special 
methods. 

All algorithms use a spatial algebra proposed by Rodriguez.

For description a robot we use a list structures proposed by Authors.

We put all robot parameters in lists with special rules.

Links ={Link[1], ..., Link[N]}	and

Motors ={Motor[1], ..., Motor[N]}.

Each link (and motor) is characterised by several kinematics and dynamics 
parameters and, in addition, by other links, which are connected to the 
links. Generally speaking it is necessary to distinguish between the links 
and motors from conceptual point of view. Also engineering practice decides 
to treat these items separately. 

Therefore i-th link is characterised by the following structure.

Link[i]= {no., kind, joints connected to it, prev joint, mass, inertia 
matrix, l vector, p vector, a, motors on the link}.

First item "no." denotes the number of the link. Next item "kind" 
specifies type of the joint which is associated with the i-th link. This 
falls in two categories: rotational and translational. Next item in the list 
is called "joint connected to it". It is a list of links, which are 
connected with the i-th link via joints. By making use of this list we are 
able to define the tree structure robots. In case of the last link in the 
linkage this field has value {0}. Here 0 denotes an external environment. 
Next field "prev. joint" locates the link itself in the topological 
structure because it defines which is the predecessor to the i-th link. 
"Mass" denotes the mass of the i-th link. "Inertia matrix" is inertia 
tensor of the i-th link. Next field "l vector" is a vector of the length of 
the link, which is a list of the three coordinates of this distance. This 
vector is used to build appropriate skew-symmetric transition matrix. Next 
vector, "p vector", is a distance to the centre of mass of the i-th link. 
Next field describes an offset angle or offset distance both of them result 
from the modified Denavit-Hartenberg notation. This parameter comes from the 
geometrical description of the robot. Finally, last field,  "motors on the 
link", is a list of motors which are mounted on the i-th link. Recall that 
on i-th link an arbitrary number of motors can be attached.

	A similar kind of list can be proposed for the i-th motor, namely.

Motor[i] ={no., link number, driven joint, mass, inertia matrix, l vector, p 
vector, a, G},

The items, which describe the i-th motor, are similar to those defining the 
i-th link. Here "link number" denotes link number to which the motor is 
mounted. Recall that, in general, motor's number is not the same as the 
link's number. Next item "driven joint" denotes a link number driven by the 
motor. Finally, "G", denotes the gear ratio between driven link and motor 
which does the job.



As an example we tested algorithms using robot Puma 560 parameters. It is an 
industrial robot with six degrees of freedom.  Below an example show to 
define links and motors of this robot:



(* Link definitions in lists *)

Link1={1,Rotationalz,{0},2,ml[1],LinkInertia1,

	 {Lxl1,Lyl1,Lzl1},{pxl1,pyl1,pzl1},alfal1,{}};



Link2={2,Rotationalz,{1},3,ml[2],LinkInertia2,

	 {Lxl2,Lyl2,Lzl2},{pxl2,pyl2,pzl2},alfal2,{1,2}};



Link3={3,Rotationalz,{2},BaseLink,ml[3],LinkInertia3,

	 {Lxl3,Lyl3,Lzl3},{pxl3,pyl3,pzl3},alfal3,{}};



LinkBase={4,Rotationalz,{3},BaseLink,0,IdentityMatrix[3],

	 {0,0,0},{0,0,0},0,{}};

(* Motor definitions in lists *)

Motor1={1,Rotationalz,2,1,mm[1],MotorInertia1,

	   {Lxm1,Lym1,Lzm1},{pxm1,pym1,pzm1},alfam1,gear1};



Motor2={2,Rotationalz,2,2,mm[2],MotorInertia2,

	   {Lxm2,Lym2,Lzm2},{pxm2,pym2,pzm2},alfam2,gear2};



Motor3={3,Rotationalz,BaseMotor,3,mm[3],MotorInertia3,

	   {Lxm3,Lym3,Lzm3},{pxm3,pym3,pzm3},alfam3,gear3};



(* Set of definition for all manipulator in two lists *)

Links={Link1,Link2,Link3,LinkBase};

Motors={Motor1,Motor2,Motor3};



Here we have a big problem. Because equations are stored dynamically in 
memory during calculations we don't know how much memory will be needed. So 
we have a problem with space. It is possible to solve the corresponding 
problem for four degrees of freedoms. A robot has six. If we try more than 
four we get out of memory from kernel.

Results

Package "Robo" can map dynamically equations. It is possible to view these 
equations. 

For example if we use function TraceForce we can see all equations that will 
be used to solve Forward Dynamics. Of course we use spatial algebra notation.



Trace Force 1

fmPlus[1]=Mm[1].AccMMinus[1]+bm[1]

fmMinus[1]=Am[1].fmPlus[1]

flMinus[1]=Ml[1].AccLMinus[1]+bl[1]

flPlus[1]=Al[1].flMinus[1]

                   + {0, 0, 0, 0, 0, 0}   (* External Force *)

 Tl[1]=Hl[1].flMinus[1]

 Tm[1]=Hm[1].matfmPlus[1]

      Force  1 has been done ...



Trace Force 2

fmPlus[2]=Mm[2].AccMMinus[2]+bm[2]

fmMinus[2]=Am[2].fmPlus[2]

flMinus[2]=Ml[2].AccLMinus[2]+bl[2]

                     + Fim[2][1].Am[1].fmPlus[1]

                     + Fim[2][2].Am[2].fmPlus[2]

                      + Fil[2][1].Al[1].flMinus[1]

flPlus[2]=Al[2].flMinus[2]

 Tl[2]=Hl[2].flMinus[2]

 Tm[2]=Hm[2].matfmPlus[2]

      Force  2 has been done ...



Trace Force 3

fmPlus[3]=Mm[3].AccMMinus[3]+bm[3]

fmMinus[3]=Am[3].fmPlus[3]

flMinus[3]=Ml[3].AccLMinus[3]+bl[3]

                      + Fil[3][2].Al[2].flMinus[2]

flPlus[3]=Al[3].flMinus[3]

 Tl[3]=Hl[3].flMinus[3]

 Tm[3]=Hm[3].matfmPlus[3]

      Force  3 has been done ...



In this way we can check manipulator definitions. 

Symbolical results with are generated are most memory consumes. These 
equations without optimizations have a lot of pages, over one hundred. We try 
to use an optimization package called "Optimize". Results are very good. 
Now equations have only three pages. Without optimizations time cost was 
about fifty minutes. Now after this and other optimizations time consumption 
is about 3 comma 5 minutes.



In our package we map all equations and next we calculate a numerical result. 
We use the following method:

(* generate numerical results *)

GenerateAccelerations[]:=

  Module[{a,b,bN,aa,bb,MatQbisGAL,MatQbisGAM},

	  Clear[AccLTable,AccMTable];

	  Print[""];

	  Print["Generating new accelerations tables ..."];

	  Print[""];

	  AccLTable	=Array[aa,{SampleNumber,LastJoint}];

	  AccMTable	=Array[bb,{SampleNumber,LastJoint}];



	    For[b=LastJoint,b>0,b--,

	      MatQbisGAL[b]=Optimize[NewS[MatQbis[b][[2,1]]] ];

	      MatQbisGAM[b]=Optimize[NewS[MatQbis[b][[1,1]]] ];

	    ];

	  For[a=1,a<=SampleNumber,a++,

	    For[b=LastJoint,b>0,b--,

	      Ql[b]	=PosLTable[[a]][[LastJoint-b+1]];

	      Qpl[b]=VelLTable[[a]][[LastJoint-b+1]]; 

	      Tm[b]	=TorqueMTable[[a]][[LastJoint-b+1]]*gear[b];

	      Tl[b]	=TorqueLTable[[a]][[LastJoint-b+1]];

	      Qm[b]	=PosMTable[[a]][[LastJoint-b+1]];

	      Qpm[b]=VelMTable[[a]][[LastJoint-b+1]]; 



	    ];   

	    For[b=1,b<=LastJoint,b++,

	      AccLTable[[a,LastJoint-b+1]]=N[ReleaseHold[MatQbisGAL[b]] ];

	      AccMTable[[a,LastJoint-b+1]]=N[ReleaseHold[MatQbisGAM[b]] ];

	    ];

	  ];

 Clear[Ql,Qpl,Tl,Qm,Qpm,Tm];

];(*GenerateAccelerations*)



But calculations for more than four degrees are most difficult. We have 
problem with memory. This problem occurs only with very complicated task with 
a lot of equations with recursions.

Summary

We are looking for other methods to optimize the mapped equations. Memory 
occupancy is the most crucial problem. Time consumption during mapping is OK. 
In future we plan to adohess   have solving all possible manipulator 
structures. Now with our memory problem it is not possible.



This text will be available on www:



www.ar-kari.put.poznan.pl/~bienkows

Example for Robo

<<robo.m



	gear1:=53.69;	gear2:=107.36;	gear3:=62.61;

	alfal1:= 0  ;	alfal2:=-Pi/2;	alfal3:=0;

	alfam1:=Pi/2;	alfam2:=-Pi/2;	alfam3:=0;

	Lxl1=0.02032;	Lxl2=0.4318 ;	Lxl3=0;

	Lyl1=0.43180;	Lyl2=0.0    ;	Lyl3=0;

	Lzl1=0.0    ;	Lzl2=0.12446;	Lzl3=0;

	Lxm0=0.4318 ;	Lxm1=0.0    ;	Lxm2=0;

	Lym0=-0.3   ;	Lym1=0.3    ;	Lym2=0;

	Lzm0=0.0    ;	Lzm1=0.0    ;	Lzm2=0.4;

	pxl1:=0.0  ; 	pxl2:=0.07;	pxl3:=0.0;

	pyl1:=0.143; 	pyl2:=0.0 ;	pyl3:=0.3088;

	pzl1:=0.0  ; 	pzl2:=0.0 ;	pzl3:=0.0;

	pxm1:=0.0  ; 	pxm2:=0.0 ;	pxm3:=0.0;

	pym1:=0.0  ; 	pym2:=0.0 ;	pym3:=0.0;

	pzm1:=0.0  ; 	pzm2:=0.0 ;	pzm3:=0.0;

	ml[1]:=6.04;	ml[2]:=17.4; 	ml[3]:= 8.7;

	mm[1]:=0.8 ;	mm[2]:=1.2 ;	mm[3]:= 1.0;

	Ixxl1:=0.3155; 	Ixxl2:=0.13  ; 	Ixxl3:=0.8316;

	Ixyl1:=0.002 ; 	Ixyl2:=0.005 ; 	Ixyl3:=0.001 ;

	Iyxl1:=Ixyl1;	Iyxl2:=Ixyl2;	Iyxl3:=Ixyl3;

	Ixzl1:=0.003 ; 	Ixzl2:=0.003 ; 	Ixzl3:=0.001 ;

	Iyyl1:=0.006 ; 	Iyyl2:=0.6092; 	Iyyl3:=0.0020;

	Iyzl1:=0.006 ; 	Iyzl2:=0.006 ; 	Iyzl3:=0.001 ;

	Izxl1:=Ixzl1;	Izxl2:=Ixzl2;	Izxl3:=Ixzl3;

	Izyl1:=Iyzl1;	Izyl2:=Iyzl2;	Izyl3:=Iyzl3;

	Izzl1:=0.3355; 	Izzl2:=0.6243; 	Izzl3:=1.1796;

	Ixxm1:=0.0   ; 	Ixxm2:=0.0   ; 	Ixxm3:=0.0   ;

	Ixym1:=0.0   ; 	Ixym2:=0.0   ; 	Ixym3:=0.0   ;

	Ixzm1:=0.0   ; 	Ixzm2:=0.0   ; 	Ixzm3:=0.0   ;

	Iyxm1:=0.0   ; 	Iyxm2:=0.0   ; 	Iyxm3:=0.0   ;

	Iyym1:=0.0   ; 	Iyym2:=0.0   ; 	Iyym3:=0.0   ;

	Iyzm1:=0.0   ; 	Iyzm2:=0.0   ; 	Iyzm3:=0.0   ;

	Izxm1:=Ixzm1;	Izxm2:=Ixzm2;	Izxm3:=Ixzm3;

	Izym1:=Iyzm1;	Izym2:=Iyzm2;	Izym3:=Iyzm3;

	Izzm1:=0.830/(gear1*gear1);

 	Izzm2:=4.71/(gear2*gear2);

 	Izzm3:=1.14/(gear3*gear3);



(* Define symbolical Inertia Matrix *)

 LinkInertia1=FormInertiaMatrix[l,1];

 LinkInertia2=FormInertiaMatrix[l,2];

 LinkInertia3=FormInertiaMatrix[l,3];



 MotorInertia1=FormInertiaMatrix[m,1];

 MotorInertia2=FormInertiaMatrix[m,2];

 MotorInertia3=FormInertiaMatrix[m,3];



SetJointSetings[1,3,4];

SetMotorSetings[1,3,4];

BaseLink=4;

BaseMotor=4;



externalforce={0,0,0,0,0,0};

(* Set external force in first join *)

SetExternalForce[{1},{externalforce}];



(* Link definitions in lists *)

Link1={1,Rotationalz,{0},2,ml[1],LinkInertia1,

	 {Lxl1,Lyl1,Lzl1},{pxl1,pyl1,pzl1},alfal1,{}};



Link2={2,Rotationalz,{1},3,ml[2],LinkInertia2,

	 {Lxl2,Lyl2,Lzl2},{pxl2,pyl2,pzl2},alfal2,{1,2}};



Link3={3,Rotationalz,{2},BaseLink,ml[3],LinkInertia3,

	 {Lxl3,Lyl3,Lzl3},{pxl3,pyl3,pzl3},alfal3,{}};



LinkBase={4,Rotationalz,{3},BaseLink,0,IdentityMatrix[3],

	 {0,0,0},{0,0,0},0,{}};

(* Motor definitions in lists *)

Motor1={1,Rotationalz,2,1,mm[1],MotorInertia1,

	   {Lxm1,Lym1,Lzm1},{pxm1,pym1,pzm1},alfam1,gear1};



Motor2={2,Rotationalz,2,2,mm[2],MotorInertia2,

	   {Lxm2,Lym2,Lzm2},{pxm2,pym2,pzm2},alfam2,gear2};



Motor3={3,Rotationalz,BaseMotor,3,mm[3],MotorInertia3,

	   {Lxm3,Lym3,Lzm3},{pxm3,pym3,pzm3},alfam3,gear3};



(* Set of definition for all manipulator in two lists *)

Links={Link1,Link2,Link3,LinkBase};

Motors={Motor1,Motor2,Motor3};



(* Set manipulator to Private local Package *)

SetManipulator[Links,Motors];



(* Define experiment parameters *)

(* numbers of sample      *)

(* experiment time        *)

SetSample[100,4];



(* bordering definition for used spatial matrixes *)

InitialMatrix[];



ComputeForwardKinematic[Links,Motors];

ComputeInverseDynamics[Links,Motors];



TrajectoryPlanning["Puma.trj"];

(*

ShowPositionsL[1];

ShowPositionsL[2];

ShowPositionsL[3];

ShowPositionsM[1];

ShowPositionsM[2];

ShowPositionsM[3];

ShowVelocityL[1];

ShowVelocityL[2];

ShowVelocityL[3];

ShowVelocityM[1];

ShowVelocityM[2];

ShowVelocityM[3];

ShowAccelerationL[1];

ShowAccelerationL[2];

ShowAccelerationL[3];

ShowAccelerationM[1];

ShowAccelerationM[2];

ShowAccelerationM[3];



SaveToDiskPR["PosL.d","VelL.d","AccL.d","PosM.d","VelM.d","AccM.d"
];

*)



GenerateForce[];



TraceForce[Links,Motors,1];

TraceForwardKinematic[Links,Motors];



(*

LoadFromDiskFR["TorqueL.d","TorqueM.d"];

*)

(*

ShowForceL[1];

ShowForceL[2];

ShowForceL[3];

ShowForceM[1];

ShowForceM[2];

ShowForceM[3];

*)

SaveToDiskFR["TorqueL.d","TorqueM.d"];

ForwardDynamics[Links,Motors];

GenerateAccelerations[];



ShowAccelerationL[1];

ShowAccelerationL[2];

ShowAccelerationL[3];

ShowAccelerationM[1];

ShowAccelerationM[2];

ShowAccelerationM[3];