1
$\begingroup$

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

$\endgroup$
  • $\begingroup$ Welcome to Robotics Titouan, but I'm afraid that it is not clear what you are asking. Are you pointing out a bug in that simulation library? Why do you need your robot to be in free-fall? What do you mean "the acceleration is false"? Please take a look at How to Ask & tour for more information on how stack exchange works and work through the Robotics question checklist to edit your question to make it clearer. $\endgroup$ – Ben Aug 18 at 15:20
  • $\begingroup$ Hello @Ben, I have edited my post. Could you tell me if it is more clear ? Thanks. $\endgroup$ – Titouan Aug 27 at 20:37
  • 2
    $\begingroup$ I guess Featherstone's library has been sufficiently tested so it must be my inputs. So what is wrong with my inputs? What are your inputs? Could you please edit your post to include the Matlab commands you're using to generate that output? $\endgroup$ – Chuck Aug 30 at 19:05
  • $\begingroup$ It's done ! @Chuck $\endgroup$ – Titouan Sep 3 at 16:35
2
$\begingroup$

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
  1. You have a Cartesian velocity, and
             0
       -1.0000
       -1.0000
  1. 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.

$\endgroup$
  • $\begingroup$ Hello @Chuck, the acceleration I get is the same as you, I corrected it yesterday I had left one calculated with another initial velocity. Maybe you are right and the acceleration I instinctively want to put equal to gravity is the 'material acceleration' and the 'spatial acceleration' returned by Featherstone algorithms is still correct (take a look at Spatial acceleration - Wikipedia). I'll study them both and edit my post to clarify the difference between the two. $\endgroup$ – Titouan Sep 5 at 14:34
  • $\begingroup$ @Titouan - Yeah, I think the last line on that Wikipedia page sums up exactly what you're seeing - "In general, the spatial acceleration $\vec{\psi}_P$ of a particle point $P$ that is moving with linear velocity $\vec{v}_P$ is derived from the material acceleration $\vec{a}_P$ at $P$ as: $\vec{\psi}_P = \vec{a}_P - \vec{\omega} \times \vec{v}_P$." I had speculated earlier that the discrepancy is "Coriolis forces or some other fictional force," and it's pretty close to to that; Coriolis force is $-2 m \omega \times v$. $\endgroup$ – Chuck Sep 5 at 15:06
  • $\begingroup$ So, not exactly a Coriolis force, but it's a similar form. Also as previously mentioned, I was never able to conceptualize the spatial math myself, so it made applying the math very difficult. Hopefully this is enough to help you troubleshoot on your own, but I'm afraid I can't provide any help beyond this. $\endgroup$ – Chuck Sep 5 at 15:09

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.