Free-floating sphere dynamics using Roy Featherstone's spatial_v2 toolbox

Question

I am using Roy Featherstone's spatial_v2 to model a mobile robot. I encountered a problem so I went back to modeling a free-floating sphere to familiarize myself with the library and the algorithms.

I have an error when I model the sphere in free fall in the zy plane while spinning around x. I am using the FDfb function for the forward dynamics as the sphere is a free-floating object.

The spatial velocity, in fixed based coordinates, is set to : $$v = \begin{bmatrix} -5\\ 0\\ 0\\ 0\\ -1 \\-1 \end{bmatrix}$$ And being in free fall and with no centrifugal force, the spatial acceleration should be, in fixed based coordinates :
$$a = \begin{bmatrix} 0\\ 0\\ 0\\ 0\\ 0 \\-9.8066 \end{bmatrix}$$ But the one returned by the FDfb function is, in fixed based coordinates : $$a = \begin{bmatrix} 0\\ 0\\ 0\\ 0\\ 5\\-14.8066 \end{bmatrix}$$

I guess Featherstone's library has been sufficiently tested so it must be my inputs. So what is wrong with my inputs ? Could someone else test it and post the results ?

sphere.NB     = 1;
sphere.parent = 0;
sphere.jtype  = {'R'}; 
sphere.Xtree  = {eye(6)};
% Inertia
M = 0.2;
C = [0 0 0];
J = diag([0.0002 0.0002 0.0002]);
sphere.I = {mcI( M, C, J )};
sphere.gravity = [0 0 -9.8066]';                                         
% To Floating base 
sphere = floatbase(sphere);
% Simulation parameters
vv_0  = [-5 0 0 0 -1 -1]';
xfb = [rq(eye(3)); [0 0 0]'; vv_0]; 

% Forward Dynamic
[xdfb] = FDfb(sphere, xfb, [], [], [])

I hope one of you can help me understand. Thanks !
Titouan

Answer 1

You still haven't posted the (full) code that gives the results you've presented; when I run your snippet I don't the results you posted. Instead, I get:

[xdfb] = FDfb(sphere, xfb, [], [], [])

xdfb =

         0
   -2.5000
         0
         0
         0
   -1.0000
   -1.0000
         0
         0
         0
         0
    5.0000
  -14.8066

Here xdfb is the time-derivative of your input system sphere and the initial conditions xfb. It's a 13-element vector made up of the derivative of the things that comprise xfb, which means:

1. You have a quaternion derivative,

             0
       -2.5000
             0
             0

2. You have a Cartesian velocity, and

             0
       -1.0000
       -1.0000

3. You have a spatial acceleration.

             0
             0
             0
             0
        5.0000
      -14.8066

Note that here the spatial acceleration I have is not what you've presented in your question, so I'm not positive that we're doing the same thing.

You can use constant-acceleration equations to calculate future positions. For your y-values, there's no acceleration, so:

$$ y = y_0 + v_y \Delta t + \frac{1}{2} a_y \Delta t^2 \\ $$

becomes

$$ y = y_0 + v_y \Delta t \\ $$

For your starting position of $y_0 = 0$ and starting speed of $v_y = -1$, after 30 seconds you should have:

$$ y = 0 + (-1)*30 \\ $$

for the trivial result of $y = -30$.

For z, you have a constant acceleration of gravity and a starting speed of -1, so you should have:

$$ z = 0 + (-1)*30 + (0.5)*(-9.8066)*(30^2) \\ $$

for a result of $z = -4,443$.

If I run the following after running your code:

t0 = 0;
tMax = 30;
dT = 0.001;
time = t0:dT:tMax;

nSamples = numel(time);

pos = zeros(3,nSamples);

for i=1:nSamples
    % Forward Dynamic
    [xdfb] = FDfb(sphere, xfb, [], [], []);
    q = xfb(1:4);    
    dq = xdfb(1:4);

    r = xfb(5:7);
    v = xdfb(5:7);

    v_old = xfb(8:13);
    a = xdfb(8:13);

    q = q + dq*dT;
    q = q./(norm(q)); % re-normalize the unit quaternion
    r = r + v*dT;
    v = v_old + a*dT;

    pos(:,i) = r;
    xfb = [q;r;v];
end
plot3(pos(1,:),pos(2,:),pos(3,:));

Then I get:

>> pos(:,end)

ans =

   1.0e+03 *

         0
   -0.0300
   -4.4431

There's a slight difference in the z-value from integration/rounding errors, but the y-value is correct. There's no problem in the output.

I think the issue for you is kind of the same issue I had with the spatial vector formulation - it was really hard for me to conceptualize. I studied Featherstone's method for a while on my own and eventually gave up because of difficulties I had in trying to implement anything based on his work. I didn't (don't) have any formal classroom training on his spatial maths and so didn't have anyone to ask for help and didn't have the ability to get feedback on whether anything I was doing was correct or not.

You've given the sphere a rotational speed of -5 about the x-axis, and this is how you're getting a y-axis term in the spatial acceleration.

Frankly speaking, I've never had any education with regards to screw theory, Plücker coordinates, etc., so again I don't have any intuition here to be able to illuminate anything more for you. I'd like to say it's Coriolis forces or some other fictional force, but it's all a guess for me.

You can look at the source code for FDfb and see the conversion from your spatial velocities to local floating-body velocities. When I plot them, everything looks as expected - the sphere is spinning, so I see the speed oscillating between y- and z-axes, and it's in free-fall, so I see the magnitude of speed (on both axes) increasing.

So, tl;dr - I can't explain how the spatial velocities or accelerations are working, but the orientations and positions should all be coming out correct.