# Non-planar robot Dynamics error using Roy Featherstone's Spatial toolbox

I am using this code to test robot dynamics of above robot using Spatial Toolbox function:

FDab(rob, [0;0;0], [0;0;0], [0;0;0]) %a_grav = [0,0,-9.81]


But it gives error:

Error using  *
Inner matrix dimensions must agree.
Error in FDab (line 49)
a{i} = Xup{i} * -a_grav + c{i};


This is because Xup is 6x6 double whereas a_grav is 1x3.

The 3 link non-planar robot used is given as:

rob.NB = 3;
rob.parent = [0:2];
rob.jtype = { 'R', 'R', 'R' }
model.gravity = [0 0 0];
rob.Xtree{1} = rotx(1.57) * xlt([0 0 0]);
rob.Xtree{2} = roty(1.57) * xlt([0.15,0,0]);
rob.Xtree{3} = xlt([0.34 0 0]);
ax1=0.03; ay1=0.03; az1=0.03;
ax2=0.28; ay2=0.05; az2=0.05;
ax3=0.2; ay3=0.05; az3=0.05;
rob.I{1} = mcI( 1, [0 0 -0.02], 1/12*[ay1^2+az1^2 0 0; 0 ax1^2+az1^2 0; 0 0 ax1^2+ay1^2] )
rob.I{2} = mcI( 4, [0.14 0 0], 4/12*[ay2^2+az2^2 0 0; 0 ax2^2+az2^2 0; 0 0 ax2^2+ay2^2] )
rob.I{3} = mcI( 3, [0.1 0 0], 3/12*[ay3^2+az3^2 0 0; 0 ax3^2+az3^2 0; 0 0 ax3^2+ay3^2] )
rob.appearance.base = ...
{ 'box', [-0.2 -0.3 -0.2; 0.2 0.3 -0.07] };
rob.appearance.body{1} = ...
{ 'cyl', [0 0 0; 0.11 0 0], 0.05, ...
'cyl', [0 0 -0.06; 0 0 0.06], 0.06 };
rob.appearance.body{2} = ...
{ 'cyl', [0 0 0; 0.34 0 0], 0.05, ...
'cyl', [0 0 -0.06; 0 0 0.06], 0.06 };
rob.appearance.body{3} = ...
{ 'cyl', [0 0 0; 0.26 0 0], 0.05, ...
'cyl', [0 0 -0.06; 0 0 0.06], 0.06 };
showmotion(rob)


How can I overcome the error?

I cannot recreate your issue, but you haven't posted all the steps you're taking, so I'm not sure if I'm following exactly what you've done.

What version are you using? I have Spatial_v2, and the README says (C) 2012. You have:

Error using  *
Inner matrix dimensions must agree.
Error in FDab (line 49)
a{i} = Xup{i} * -a_grav + c{i};


But that line isn't line 49 in my version of FDab, it's line 47 in my code.

Regardless, note that the FDab function doesn't get model.gravity directly; it calls get_gravity(model) and that function will return a 6x1 vector if the check size(model.Xtree{1},1) == 3 returns FALSE (lines 18 and 21 in get_gravity())

This could be where you're having the issue - you're saying your robot is non-planar, but it's passing the planar check size(model.Xtree{1},1) == 3 in get_gravity(). This is the only way you get a_grav as a 3x1 vector from get_gravity().

I don't know how you initialized your robot, but I managed to run your code without issue if I took the following steps:

1. Not provided in the OP: rob = singlebody;
2. I'm not sure if the above is correct because I had to manually empty appearance.body in order to stop errors when running your code: rob.appearance.body = []
3. Copy/paste your code from rob.NB = 3; to showmotion(rob); I got no errors and the animation played correctly.
4. You don't set gravity in the code snippet you provided, so I set: rob.gravity = [0,0,-9.81]
5. I ran the code in question:
>> FDab(rob, [0;0;0], [0;0;0], [0;0;0])

ans =

-59.1867
-3.8828
6.9148


I would guess you've got an issue with your model declaration or configuration as there are steps required to get your snippet to run. Try clear all to flush your session variables and re-running the code. It might be something you had tried previously is still in memory and that's screwing up your results.

• Thanks a lot! I was writing gravity as: rob.gravity = [0,0,-9.81] but now I have changed it to: a_grav = get_gravity(rob); and now it is working fine. – Capri Jul 23 '19 at 6:16