Forward Kinematics of non-planar robot using Roy Featherstone's Spatial Toolbox

Question

I have a non-floatbase 3 DOF non-planar robot whose forward kinematics I need to compute using Spatial Toolbox. The code for this purpose is p = fbkin(q) but as my robot is 3 DOF (q is 3x1 vector e.g. [0;0;0]) and non-floatbase, I am unable to get kinematics of the model and get this error:

fbkin([0;0;0]) %input

Index exceeds matrix dimensions.

Error in fbkin>fwdkin (line 49)
c4 = cos(q(4));  s4 = sin(q(4));

Error in fbkin (line 41)
    o1 = fwdkin( i1 );

My robot is:

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 use the fbkin function with 3x1 joint angle vector?

Answer 1

You've got to open the functions you're trying to use and look through them if you're getting errors. You said you are trying to get the forward kinematics of your robot, and that it's a non-floatbase, and that you've got a 3x1 vector of joint values q.

Yet you're using fbkin which, if you type help fbkin into the console, will state (added bold for things relevant to you):

>> help fbkin

fbkin Forward and Inverse Kinematics of Floating Base

[x,xd]=fbkin(q,qd,qdd) calculates the forward kinematics, and [q,qd,qdd]=fbkin(x,xd) calculates the inverse kinematics, of a spatial floating base constructed by floatbase.

So, right away, if you didn't use floatbase to make your base, then you're using the wrong function. The rest of the help message continues:

The vectors q, qd and qdd are the joint position, velocity and acceleration variables for the six joints forming the floating base; x is the 13-element singularity-free state vector used by FDfb and IDfb; and xd is its derivative.

And here it says it's a floating base, so naturally it's got six degrees of freedom, so if you're going to do anything with it then you can imagine the base as having six joints (x/y/z roll/pitch/yaw) and that's what this function is looking for when it uses q as an input. It is not saying that this is for use with a robot arm or a kinematic tree or anything else. Just the floating base.

Continuing:

The component parts of x are: a unit quaternion specifying the orientation of the floating-base frame relative to the fixed base, a 3D vector giving the position of the origin of the floating-base frame, and the spatial velocity of the floating base expressed in fixed-base coordinates.

That is the output for the forward kinematics code - quaternion orientation and x/y/z position, and spatial velocity, which is angular and linear velocities. The remainder of the help comment talks about different ways to call the function and things to look out for with regards to singularities.

This is not the code that's going to do forward kinematics for a robot arm. It's doing kinematics for a base.

You need to go to the \dyanmics folder and look at some of the FD__ functions; those are the Forward Dynamics functions. Actually I was going to suggest you try FDab and I remembered that was at the core of your other question, so I'm wondering why you're not using it.

Actually, in re-reading your question, it seems like you're not actually trying to do any dynamics simulations, rather you just seem to want to plug in a vector of joint values and get positions. In that case, I think the command you're looking for is jcalc. This function takes a joint type and joint value q and returns the spatial transform Xj (which is a 6x6 matrix and not a 4x4 homogenous transform!).

Function like FDab call jcalc to build a chain of transforms, as in:

for i = 1:model.NB
  [ XJ, S{i} ] = jcalc( model.jtype{i}, q(i) );
  vJ = S{i}*qd(i);
  Xup{i} = XJ * model.Xtree{i};

But I don't think there's any one function that's going to take your q vector and spit out a series of positions. You'll have to build your own with jcalc and work from there.